simulator_ignition_bridge: wait for first clock set before returning successfully

platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_scheduler.cpp

@@ -33,6 +33,8 @@
 
 #include <lockstep_scheduler/lockstep_scheduler.h>
 
+#include <px4_platform_common/log.h>
+
 LockstepScheduler::~LockstepScheduler()
 {
 	// cleanup the linked list
@@ -47,6 +49,10 @@ LockstepScheduler::~LockstepScheduler()
 
 void LockstepScheduler::set_absolute_time(uint64_t time_us)
 {
+	if (_time_us == 0 && time_us > 0) {
+		PX4_INFO("setting initial absolute time to %lu us", time_us);
+	}
+
 	_time_us = time_us;
 
 	{

src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp

@@ -221,7 +221,32 @@ int SimulatorIgnitionBridge::task_spawn(int argc, char *argv[])
 		_task_id = task_id_is_work_queue;
 
 		if (instance->init() == PX4_OK) {
+
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+			// lockstep scheduler wait for initial clock set before returning
+			int sleep_count_limit = 1000;
+
+			while ((instance->world_time_us() == 0) && sleep_count_limit > 0) {
+				// wait for first clock message
+				system_usleep(1000);
+				sleep_count_limit--;
+			}
+
+			if (instance->world_time_us() == 0) {
+				PX4_ERR("timed out waiting for clock message");
+				instance->request_stop();
+				instance->ScheduleNow();
+
+			} else {
+				return PX4_OK;
+			}
+
+#else
 			return PX4_OK;
+
+#endif // ENABLE_LOCKSTEP_SCHEDULER
+
+			//return PX4_OK;
 		}
 
 	} else {

src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.hpp

@@ -79,6 +79,8 @@ public:
 	/** @see ModuleBase::print_status() */
 	int print_status() override;
 
+	uint64_t world_time_us() const { return _world_time_us.load(); }
+
 private:
 
 	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],