Automatically correct elevation of objects initialized on elevation maps

modules/simulator/include/mvsim/VehicleBase.h

@@ -29,6 +29,7 @@
 #include <map>
 #include <mutex>
 #include <string>
+#include <atomic>
 
 #include "CsvLogger.h"
 
@@ -213,6 +214,7 @@ class VehicleBase : public VisualObject, public Simulable
 	std::vector<mrpt::opengl::CSetOfObjects::Ptr> glWheelsViz_, glWheelsPhysical_;
 	mrpt::opengl::CSetOfLines::Ptr glForces_;
 	mrpt::opengl::CSetOfLines::Ptr glMotorTorques_;
+	std::atomic_bool glInit_ = false;
 
 	std::vector<mrpt::math::TSegment3D> forceSegmentsForRendering_;
 	std::vector<mrpt::math::TSegment3D> torqueSegmentsForRendering_;

modules/simulator/include/mvsim/World.h

@@ -390,8 +390,9 @@ class World : public mrpt::system::COutputLogger
 	 */
 	std::set<float> getElevationsAt(const mrpt::math::TPoint2Df& worldXY) const;
 
-	/// with query points the center of a wheel, this returns the highest "ground" under it.
-	std::optional<float> getHighestElevationUnder(const mrpt::math::TPoint3Df& queryPt) const;
+	/// with query points the center of a wheel, this returns the highest "ground" under it, or .0
+	/// if nothing found.
+	float getHighestElevationUnder(const mrpt::math::TPoint3Df& queryPt) const;
 
 	void internal_simul_pre_step_terrain_elevation();
 

modules/simulator/src/Simulable.cpp

@@ -182,6 +182,7 @@ bool Simulable::parseSimulable(const JointXMLnode<>& rootNode, const ParseSimula
 	// -------------------------------------
 	// (Mandatory) initial pose:
 	// -------------------------------------
+	std::optional<mrpt::math::TPose3D> initPose;
 	if (const xml_node<>* nPose = rootNode.first_node("init_pose"); nPose)
 	{
 		mrpt::math::TPose3D p;
@@ -193,8 +194,7 @@ bool Simulable::parseSimulable(const JointXMLnode<>& rootNode, const ParseSimula
 			THROW_EXCEPTION_FMT("Error parsing <init_pose>%s</init_pose>", nPose->value());
 		p.yaw *= M_PI / 180.0;	// deg->rad
 
-		this->setPose(p);
-		initial_q_ = p;	 // save it for later usage in some animations, etc.
+		initPose = p;
 	}
 	else if (const xml_node<>* nPose3 = rootNode.first_node("init_pose3d"); nPose3)
 	{
@@ -209,14 +209,36 @@ bool Simulable::parseSimulable(const JointXMLnode<>& rootNode, const ParseSimula
 		p.pitch *= M_PI / 180.0;  // deg->rad
 		p.roll *= M_PI / 180.0;	 // deg->rad
 
-		this->setPose(p);
-		initial_q_ = p;	 // save it for later usage in some animations, etc.
+		initPose = p;
 	}
 	else if (psp.init_pose_mandatory)
 	{
 		THROW_EXCEPTION("Missing required XML node <init_pose>x y phi</init_pose>");
 	}
 
+	if (const rapidxml::xml_node<char>* xml_skip_elevation_adjust =
+			rootNode.first_node("skip_elevation_adjust");
+		initPose && xml_skip_elevation_adjust == nullptr)
+	{
+		// "skip" tag is NOT present => Do adjustment:
+
+		// Query: the highest object point:
+		auto queryPt = initPose->translation();
+		// TODO: Automatic determination:
+		queryPt.z += 1.5;  // [m]
+
+		if (std::optional<float> elev = simulable_parent_->getHighestElevationUnder(queryPt); elev)
+		{
+			initPose->z += elev.value();
+		}
+	}
+
+	if (initPose)
+	{
+		this->setPose(*initPose);
+		initial_q_ = *initPose;	 // save it for later usage in some animations, etc.
+	}
+
 	// -------------------------------------
 	// (Optional) initial vel:
 	// -------------------------------------

modules/simulator/src/VehicleBase.cpp

@@ -692,6 +692,8 @@ void VehicleBase::internalGuiUpdate(
 
 		viz->get().insert(glChassisViz_);
 		physical->get().insert(glChassisPhysical_);
+
+		glInit_ = true;
 	}
 
 	// Update them:
@@ -701,10 +703,8 @@ void VehicleBase::internalGuiUpdate(
 	// need/can't acquire it again:
 	const auto objectPose = viz.has_value() ? getPose() : getPoseNoLock();
 
-	if (glChassisViz_)
+	if (glInit_)
 	{
-		ASSERT_(glChassisPhysical_);
-
 		glChassisViz_->setPose(objectPose);
 		glChassisPhysical_->setPose(objectPose);
 		for (size_t i = 0; i < nWs; i++)

modules/simulator/src/World.cpp

@@ -289,13 +289,14 @@ std::set<float> World::getElevationsAt(const mrpt::math::TPoint2Df& worldXY) con
 	return ret;
 }
 
-std::optional<float> World::getHighestElevationUnder(const mrpt::math::TPoint3Df& pt) const
+float World::getHighestElevationUnder(const mrpt::math::TPoint3Df& pt) const
 {
 	const auto zs = getElevationsAt({pt.x, pt.y});
 
 	auto it = zs.upper_bound(pt.z);
 	if (it == zs.begin()) return 0.0f;	// No element <= threshold
 
 	--it;
+
 	return *it;
 }

modules/simulator/src/World_simul.cpp

@@ -339,8 +339,10 @@ void World::internal_simul_pre_step_terrain_elevation()
 			mrpt::math::TPose3D new_pose = cur_pose;
 			corrs.clear();
 
-			bool out_of_area = false;
-			for (size_t iW = 0; !out_of_area && iW < nWheels; iW++)
+			bool all_equal = true;
+			std::optional<float> equal_zs;
+
+			for (size_t iW = 0; iW < nWheels; iW++)
 			{
 				const Wheel& wheel = veh->getWheelInfo(iW);
 
@@ -357,32 +359,39 @@ void World::internal_simul_pre_step_terrain_elevation()
 					gPt + mrpt::math::TPoint3D(.0, .0, .5 * wheel.diameter);
 
 				// Get "the ground" under my wheel axis:
-				const auto z = this->getHighestElevationUnder(gPtWheelsAxis);
-				if (!z.has_value())
-				{
-					out_of_area = true;
-					continue;  // vehicle is out of bounds!
-				}
+				const float z = this->getHighestElevationUnder(gPtWheelsAxis);
+
+				if (!equal_zs) equal_zs = z;
+				if (std::abs(*equal_zs - z) > 1e-4) all_equal = false;
 
 				corr.globalIdx = iW;
-				corr.global = mrpt::math::TPoint3D(gPt.x, gPt.y, *z);
+				corr.global = mrpt::math::TPoint3D(gPt.x, gPt.y, z);
 
 				corrs.push_back(corr);
-			}
-			if (out_of_area) continue;
+			}  // end for each Wheel
 
-			// Register:
-			double transf_scale;
-			mrpt::poses::CPose3DQuat tmpl;
+			if (all_equal && equal_zs.has_value())
+			{
+				// Optimization: just use the constant elevation without optimizing:
+				new_pose.z = optimalTf.z();
+				new_pose.pitch = 0;
+				new_pose.roll = 0;
+			}
+			else if (corrs.size() >= 3)
+			{
+				// Register:
+				double transf_scale;
+				mrpt::poses::CPose3DQuat tmpl;
 
-			mrpt::tfest::se3_l2(corrs, tmpl, transf_scale, true /*force scale unity*/);
+				mrpt::tfest::se3_l2(corrs, tmpl, transf_scale, true /*force scale unity*/);
 
-			optimalTf = mrpt::poses::CPose3D(tmpl);
+				optimalTf = mrpt::poses::CPose3D(tmpl);
 
-			new_pose.z = optimalTf.z();
-			new_pose.yaw = optimalTf.yaw();
-			new_pose.pitch = optimalTf.pitch();
-			new_pose.roll = optimalTf.roll();
+				new_pose.z = optimalTf.z();
+				new_pose.yaw = optimalTf.yaw();
+				new_pose.pitch = optimalTf.pitch();
+				new_pose.roll = optimalTf.roll();
+			}
 
 			veh->setPose(new_pose);
 

modules/simulator/src/parse_utils.cpp

@@ -27,7 +27,9 @@ thread_local const bool MVSIM_VERBOSE_PARSE = mrpt::get_env<bool>("MVSIM_VERBOSE
 
 using namespace mvsim;
 
-static std::string parseEnvVars(const std::string& text)
+namespace
+{
+std::string parseEnvVars(const std::string& text)
 {
 	MRPT_TRY_START
 
@@ -60,7 +62,7 @@ static std::string parseEnvVars(const std::string& text)
 	MRPT_TRY_END
 }
 
-static std::string parseVars(
+std::string parseVars(
 	const std::string& text, const std::map<std::string, std::string>& variableNamesValues)
 {
 	MRPT_TRY_START
@@ -103,7 +105,7 @@ static std::string parseVars(
 	MRPT_TRY_END
 }
 
-static std::string parseCmdRuns(const std::string& text)
+std::string parseCmdRuns(const std::string& text)
 {
 	MRPT_TRY_START
 
@@ -140,26 +142,24 @@ static std::string parseCmdRuns(const std::string& text)
 	MRPT_TRY_END
 }
 
-#if MRPT_VERSION >= 0x258
-static double my_rand()
+double my_rand()
 {
 	auto& rng = mrpt::random::getRandomGenerator();
 	return rng.drawUniform(0.0, 1.0);
 }
-static double my_unifrnd(double xMin, double xMax)
+double my_unifrnd(double xMin, double xMax)
 {
 	auto& rng = mrpt::random::getRandomGenerator();
 	return rng.drawUniform(xMin, xMax);
 }
-static double randn()
+double randn()
 {
 	auto& rng = mrpt::random::getRandomGenerator();
 	return rng.drawGaussian1D_normalized();
 }
-#endif
 
 // Examples: "$f{180/5}",   "$f{ ${MAX_SPEED} * sin(deg2rad(45)) }"
-static std::string parseMathExpr(
+std::string parseMathExpr(
 	const std::string& text, const std::map<std::string, std::string>& variableNamesValues)
 {
 	MRPT_TRY_START
@@ -182,11 +182,9 @@ static std::string parseMathExpr(
 
 	mrpt::expr::CRuntimeCompiledExpression expr;
 
-#if MRPT_VERSION >= 0x258
 	expr.register_function("rand", &my_rand);
 	expr.register_function("unifrnd", &my_unifrnd);
 	expr.register_function("randn", &randn);
-#endif
 
 	std::map<std::string, double> numericVars;
 	for (const auto& kv : variableNamesValues)
@@ -207,6 +205,7 @@ static std::string parseMathExpr(
 
 	MRPT_TRY_END
 }
+}  // namespace
 
 std::string mvsim::parse(
 	const std::string& input, const std::map<std::string, std::string>& variableNamesValues)

mvsim_tutorial/demo_elevation_map.world.xml

@@ -131,11 +131,11 @@
 	     ======================== -->
 	<variable name="ROCKS_SPREAD_X" value="35.0"></variable>
 	<variable name="ROCKS_SPREAD_Y" value="35.0"></variable>
-	<variable name="ROCKS_SPREAD_Z" value="1.5"></variable>
+	<variable name="ROCKS_SPREAD_Z" value="1.0"></variable>
 
 	<variable name="TREES_SPREAD_X" value="20.0"></variable>
 	<variable name="TREES_SPREAD_Y" value="20.0"></variable>
-	<variable name="TREES_SPREAD_Z" value="1.5"></variable>
+	<variable name="TREES_SPREAD_Z" value="0"></variable>
 
 	<for var="i" from="1" to="12">
 		<block class="rock_01">