diff --git a/modules/simulator/include/mvsim/VehicleBase.h b/modules/simulator/include/mvsim/VehicleBase.h index 7eed6c4..70a9966 100644 --- a/modules/simulator/include/mvsim/VehicleBase.h +++ b/modules/simulator/include/mvsim/VehicleBase.h @@ -29,6 +29,7 @@ #include #include #include +#include #include "CsvLogger.h" @@ -213,6 +214,7 @@ class VehicleBase : public VisualObject, public Simulable std::vector glWheelsViz_, glWheelsPhysical_; mrpt::opengl::CSetOfLines::Ptr glForces_; mrpt::opengl::CSetOfLines::Ptr glMotorTorques_; + std::atomic_bool glInit_ = false; std::vector forceSegmentsForRendering_; std::vector torqueSegmentsForRendering_; diff --git a/modules/simulator/include/mvsim/World.h b/modules/simulator/include/mvsim/World.h index 0aa0008..770240c 100644 --- a/modules/simulator/include/mvsim/World.h +++ b/modules/simulator/include/mvsim/World.h @@ -390,8 +390,9 @@ class World : public mrpt::system::COutputLogger */ std::set getElevationsAt(const mrpt::math::TPoint2Df& worldXY) const; - /// with query points the center of a wheel, this returns the highest "ground" under it. - std::optional 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(); diff --git a/modules/simulator/src/Simulable.cpp b/modules/simulator/src/Simulable.cpp index b3214e8..eeb8f34 100644 --- a/modules/simulator/src/Simulable.cpp +++ b/modules/simulator/src/Simulable.cpp @@ -182,6 +182,7 @@ bool Simulable::parseSimulable(const JointXMLnode<>& rootNode, const ParseSimula // ------------------------------------- // (Mandatory) initial pose: // ------------------------------------- + std::optional 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 %s", 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 x y phi"); } + if (const rapidxml::xml_node* 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 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: // ------------------------------------- diff --git a/modules/simulator/src/VehicleBase.cpp b/modules/simulator/src/VehicleBase.cpp index dcd7fb4..7805928 100644 --- a/modules/simulator/src/VehicleBase.cpp +++ b/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++) diff --git a/modules/simulator/src/World.cpp b/modules/simulator/src/World.cpp index 90d5219..6428840 100644 --- a/modules/simulator/src/World.cpp +++ b/modules/simulator/src/World.cpp @@ -289,7 +289,7 @@ std::set World::getElevationsAt(const mrpt::math::TPoint2Df& worldXY) con return ret; } -std::optional 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}); @@ -297,5 +297,6 @@ std::optional World::getHighestElevationUnder(const mrpt::math::TPoint3Df if (it == zs.begin()) return 0.0f; // No element <= threshold --it; + return *it; } diff --git a/modules/simulator/src/World_simul.cpp b/modules/simulator/src/World_simul.cpp index 4b5e55b..ec8551b 100644 --- a/modules/simulator/src/World_simul.cpp +++ b/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 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); diff --git a/modules/simulator/src/parse_utils.cpp b/modules/simulator/src/parse_utils.cpp index ef38cff..54b429b 100644 --- a/modules/simulator/src/parse_utils.cpp +++ b/modules/simulator/src/parse_utils.cpp @@ -27,7 +27,9 @@ thread_local const bool MVSIM_VERBOSE_PARSE = mrpt::get_env("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& 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& 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 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& variableNamesValues) diff --git a/mvsim_tutorial/demo_elevation_map.world.xml b/mvsim_tutorial/demo_elevation_map.world.xml index 8ea41cb..da9db24 100644 --- a/mvsim_tutorial/demo_elevation_map.world.xml +++ b/mvsim_tutorial/demo_elevation_map.world.xml @@ -131,11 +131,11 @@ ======================== --> - + - +