Generate ground truth trajectory files in the TUM format

docs/world_global.rst

@@ -32,6 +32,13 @@ Available parameters (all are optional):
 - ``<rawlog_odometry_rate>10.0</rawlog_odometry_rate>``: If ``save_to_rawlog`` is enabled,
   this parameter defines the rate (in Hz) to generate (wheels) odometry observations (Default is 10 Hz).
 
+- ``<save_ground_truth_trajectory>gt.txt</save_ground_truth_trajectory>``: If non-empty, the
+  ground truth trajectory of all vehicles will be saved to a text file in the 
+  `TUM trajectory format <https://github.com/MichaelGrupp/evo/wiki/Formats#tum---tum-rgb-d-dataset-trajectory-format>`_.
+  One file will be created per vehicle,  by adding the vehicle name to the provided file name.
+
+- ``<ground_truth_rate>50.0</ground_truth_rate>``: If ``save_ground_truth_trajectory`` is enabled,
+  this parameter defines the rate (in Hz) to generate entries in the trajectory file (Default is 50 Hz).
 
 .. code-block:: xml
    :caption: Top-level and global settings example

modules/simulator/include/mvsim/World.h

@@ -432,6 +432,12 @@ class World : public mrpt::system::COutputLogger
 
 	double rawlog_odometry_rate_ = 10.0;  //!< In Hz.
 
+	/** If non-empty, the ground truth trajectory of all vehicles will be saved
+	 * to a text file in the TUM trajectory format */
+	std::string save_ground_truth_trajectory_;
+
+	double ground_truth_rate_ = 50.0;  //!< In Hz.
+
 	const TParameterDefinitions otherWorldParams_ = {
 		{"server_address", {"%s", &serverAddress_}},
 		{"gravity", {"%lf", &gravity_}},
@@ -441,6 +447,9 @@ class World : public mrpt::system::COutputLogger
 		{"joystick_enabled", {"%bool", &joystickEnabled_}},
 		{"save_to_rawlog", {"%s", &save_to_rawlog_}},
 		{"rawlog_odometry_rate", {"%lf", &rawlog_odometry_rate_}},
+		{"save_ground_truth_trajectory",
+		 {"%s", &save_ground_truth_trajectory_}},
+		{"ground_truth_rate", {"%lf", &ground_truth_rate_}},
 	};
 
 	/** User-defined variables as defined via `<variable name='' value='' />`
@@ -732,12 +741,17 @@ class World : public mrpt::system::COutputLogger
 		const Simulable& veh, const mrpt::obs::CObservation::Ptr& obs);
 
 	void internalPostSimulStepForRawlog();
+	void internalPostSimulStepForTrajectory();
 
 	std::mutex rawlog_io_mtx_;
 	std::map<std::string, std::shared_ptr<mrpt::io::CFileGZOutputStream>>
 		rawlog_io_per_veh_;
 	std::optional<double> rawlog_last_odom_time_;
 
+	std::mutex gt_io_mtx_;
+	std::map<std::string, std::fstream> gt_io_per_veh_;
+	std::optional<double> gt_last_time_;
+
 	// Services:
 	void internal_advertiseServices();	// called from connectToServer()
 

modules/simulator/src/World.cpp

@@ -9,6 +9,7 @@
 #include <mrpt/core/lock_helper.h>
 #include <mrpt/math/TTwist2D.h>
 #include <mrpt/obs/CObservationOdometry.h>
+#include <mrpt/poses/CPose3DQuat.h>
 #include <mrpt/system/filesystem.h>	 // filePathSeparatorsToNative()
 #include <mrpt/version.h>
 #include <mvsim/World.h>
@@ -241,6 +242,7 @@ void World::internal_one_timestep(double dt)
 	mrpt::system::CTimeLoggerEntry tle5(timlogger_, "timestep.5.post_rawlog");
 
 	internalPostSimulStepForRawlog();
+	internalPostSimulStepForTrajectory();
 
 	tle5.stop();
 
@@ -512,3 +514,52 @@ void World::internalPostSimulStepForRawlog()
 		internalOnObservation(*veh.second, obs);
 	}
 }
+
+void World::internalPostSimulStepForTrajectory()
+{
+	using namespace std::string_literals;
+
+	if (save_ground_truth_trajectory_.empty()) return;
+
+	ASSERT_GT_(ground_truth_rate_, 0.0);
+
+	const double now = get_simul_time();
+	const double T_odom = 1.0 / ground_truth_rate_;
+
+	if (gt_last_time_.has_value() && (now - *gt_last_time_) < T_odom)
+	{
+		return;	 // not yet
+	}
+
+	gt_last_time_ = now;
+
+	// Create one entry per robot.
+	// First, create the output files if this is the first time:
+	auto lck = mrpt::lockHelper(gt_io_mtx_);
+	if (gt_io_per_veh_.empty())
+	{
+		for (const auto& [vehName, veh] : vehicles_)
+		{
+			const std::string fileName = mrpt::system::fileNameChangeExtension(
+				save_ground_truth_trajectory_, vehName + ".txt"s);
+
+			MRPT_LOG_INFO_STREAM("Creating ground truth file: " << fileName);
+
+			gt_io_per_veh_[vehName] = std::fstream(fileName, std::ios::out);
+		}
+	}
+
+	const double t = mrpt::Clock::toDouble(get_simul_timestamp());
+
+	for (const auto& [vehName, veh] : vehicles_)
+	{
+		const auto p = mrpt::poses::CPose3DQuat(veh->getCPose3D());
+
+		// each row contains these elements separated by spaces:
+		// timestamp x y z q_x q_y q_z q_w
+
+		gt_io_per_veh_.at(vehName) << mrpt::format(
+			"%f %f %f %f %f %f %f %f\n", t, p.x(), p.y(), p.z(), p.quat().x(),
+			p.quat().y(), p.quat().z(), p.quat().w());
+	}
+}

mvsim_tutorial/demo_jackal.world.xml

@@ -6,7 +6,7 @@
 	<!-- <save_to_rawlog>jackal_dataset.rawlog</save_to_rawlog> -->
 	<!-- If save_to_rawlog is enabled, this defines the rate in Hz to generate odometry observations -->
 	<!-- <rawlog_odometry_rate>20.0</rawlog_odometry_rate> -->
-
+	<!-- <save_ground_truth_trajectory>gt.txt</save_ground_truth_trajectory> -->
 
 	<!-- GUI options -->
 	<gui>