moving platform in gazebo: first steps

the good:
 - we have a platform in gazebo, created dynamically from GZBridge.
 - it has a NavSat sensor, with a working callback on the PX4 side,
   GZBridge::platformNavsatCallback. From there we can publish the info
   as uOrb and do what we want with it.

the bad:
 - the platform doesn't move yet

the ugly:
 - the platforms sdf file is currently untracked in the
   Tools/simulation/gz submodule. Without this the commit here is
   useless. Need to find a sensible spot for it.

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -68,6 +68,9 @@ GZBridge::~GZBridge()
 
 int GZBridge::init()
 {
+	// TODO make configurable in sensible way.
+	bool platform = true;
+
 	if (!_model_sim.empty()) {
 
 		// service call to create model
@@ -101,6 +104,12 @@ int GZBridge::init()
 			position->set_y(model_pose_v[1]);
 			position->set_z(model_pose_v[2]);
 
+			if (platform) {
+				// why does this only look good if way bigger than actual platform height?
+				float platform_height = 0.5;
+				position->set_z(fmaxf(platform_height, model_pose_v[2]));
+			}
+
 			gz::math::Quaterniond q(model_pose_v[3], model_pose_v[4], model_pose_v[5]);
 
 			q.Normalize();
@@ -172,6 +181,23 @@ int GZBridge::init()
 		}
 	}
 
+	if (platform) {
+		if (!createMovingPlatform()) {
+			return PX4_ERROR;
+		}
+
+		// TODO find the version of this that actually works
+		// gz::msgs::Twist twist_msg;
+		// twist_msg.mutable_linear()->set_x(1.);
+		// twist_msg.mutable_linear()->set_y(0.);
+		// twist_msg.mutable_linear()->set_z(0.);
+
+		// std::string twist_topic = "/model/" + _model_name + "/cmd_vel";
+
+		// something something _node.Advertise()?
+		// _node.Publisher.Publish(what??);
+	}
+
 	// clock
 	std::string clock_topic = "/world/" + _world_name + "/clock";
 
@@ -247,6 +273,17 @@ int GZBridge::init()
 		return PX4_ERROR;
 	}
 
+	if (platform) {
+
+		std::string platform_pose_topic = "/world/" + _world_name +
+						  "/model/flat_platform/link/platform_link/sensor/navsat_sensor/navsat";
+
+		if (!_node.Subscribe(platform_pose_topic, &GZBridge::platformNavsatCallback, this)) {
+			PX4_ERR("failed to subscribe to %s", platform_pose_topic.c_str());
+			return PX4_ERROR;
+		}
+	}
+
 	if (!_mixing_interface_esc.init(_model_name)) {
 		PX4_ERR("failed to init ESC output");
 		return PX4_ERROR;
@@ -271,6 +308,47 @@ int GZBridge::init()
 	return OK;
 }
 
+bool GZBridge::createMovingPlatform()
+{
+
+	// moving platform to launch & land from.
+
+	gz::msgs::EntityFactory req{};
+	// base dir: Tools/simulation/gz/models
+	req.set_sdf_filename("platform.sdf");
+
+	// can also set sdf string directly using req.set_sdf
+	// https://gazebosim.org/api/gazebo/6/entity_creation.html
+	// req.set_name("platform");
+	req.set_allow_renaming(false);
+
+	// position: origin (might want to adapt to model position)
+	gz::msgs::Pose *p = req.mutable_pose();
+	gz::msgs::Vector3d *position = p->mutable_position();
+	position->set_x(0.);
+	position->set_y(0.);
+	position->set_z(0.05);
+
+	// gz::math::Quaterniond q(model_pose_v[3], model_pose_v[4], model_pose_v[5]);
+
+	// orientation: unit quaternion
+	gz::msgs::Quaternion *orientation = p->mutable_orientation();
+	orientation->set_x(0.);
+	orientation->set_y(0.);
+	orientation->set_z(0.);
+	orientation->set_w(1.);
+
+	std::string create_service = "/world/" + _world_name + "/create";
+
+	if (!callEntityFactoryService(create_service, req)) {
+		PX4_ERR("EntityFactory service call failed (platform)");
+		return false;
+	}
+
+	return true;
+
+}
+
 int GZBridge::task_spawn(int argc, char *argv[])
 {
 	const char *world_name = "default";
@@ -410,6 +488,22 @@ bool GZBridge::updateClock(const uint64_t tv_sec, const uint64_t tv_nsec)
 	return false;
 }
 
+void GZBridge::platformNavsatCallback(const gz::msgs::NavSat &nav_sat)
+{
+
+	pthread_mutex_lock(&_node_mutex);
+
+	// print d n'ebugging pas
+	PX4_INFO("Moving Platform NavSat pos: lat=%.2f, lon=%.2f, alt=%.2f",
+		 nav_sat.latitude_deg(), nav_sat.longitude_deg(), nav_sat.altitude());
+
+	// TODO publish the corresponding uOrb msg. then in reality we just have
+	// to convert the mavlink msg from the ground station to exactly the
+	// same uOrb msg.
+
+	pthread_mutex_unlock(&_node_mutex);
+}
+
 void GZBridge::clockCallback(const gz::msgs::Clock &clock)
 {
 	pthread_mutex_lock(&_node_mutex);

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -116,6 +116,9 @@ private:
 	void navSatCallback(const gz::msgs::NavSat &nav_sat);
 	void laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan);
 	void laserScanCallback(const gz::msgs::LaserScan &scan);
+	void platformNavsatCallback(const gz::msgs::NavSat &pose);
+
+	bool createMovingPlatform();
 
 	/**
 	 * @brief Call Entityfactory service