prevent blocking when no valid IMU data received and controller is waiting for sitl next servo packet

[EthanNT1000]

sitl will only send next servo packet after receiving state from plugin, so if plugin is waiting for next servo packet but no valid IMU data has been received yet, it will block indefinitely. This commit adds a check for valid IMU data and received servo packet, and if neither is true and a signal has been received, it will break out of the lock-step loop to avoid blocking.

[khancyr]

that looks wrong, this will break the lockstep each time we got jitter on messaging.
Why do you want this ?

[srmainwaring]

Agree with @khancyr, would be good to have a use case to understand the motivation for the change

so if plugin is waiting for next servo packet but no valid IMU data has been received yet, it will block indefinitely.

This is intended. It is so if a breakpoint is placed in either Gazebo or ArduPilot the simulation stops. It is important this behaviour is preserved. There is an ArduPilot plugin option that permits lock-step to be disabled if desired.

[EthanNT1000]

When I pressed start button on gazebo after the Ardupilot SITL have loaded and waiting for JSON input, it stuck forever.
So I looking to the codes found out because it locked by the plugin at the beginning, there is no IMU data.
the SITL not received the current state and keep sending old frame.
This keep pkt_frame_count == this->dataPtr->fcu_frame_count always be true on ArduPilotPlugin.cc line 1603.

[srmainwaring]

Thanks @EthanNT1000, so the issue is that if Gazebo is started paused, and ArduPilot SITL is started, the simulation does not proceed when unpaused. We should check whether the order in which Gazebo and ArduPilot are started matters as well.

That is something we should fix, but this PR may not be the best approach. Let me take a look and see if we can find a solution that does not break lock-step.

[EthanNT1000]

And also the imuMsgValid is true but the JSON string is empty for some reason.
So I thought let the simulation run once to publish the IMU data.
My Simulation always started paused.
I will find out how to change it tomorrow.
Thank you for supporting.