diff --git a/modules/simulator/include/mvsim/World.h b/modules/simulator/include/mvsim/World.h index 741f894..0aa0008 100644 --- a/modules/simulator/include/mvsim/World.h +++ b/modules/simulator/include/mvsim/World.h @@ -35,6 +35,7 @@ #include #include #include +#include #if MVSIM_HAS_ZMQ && MVSIM_HAS_PROTOBUF // forward declarations: @@ -387,7 +388,12 @@ class World : public mrpt::system::COutputLogger * always reported by default. In multistorey worlds, for example, this will return the height * of each floor for the queried point. */ - std::vector getElevationsAt(const mrpt::math::TPoint2Df& worldXY) const; + 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; + + void internal_simul_pre_step_terrain_elevation(); private: friend class VehicleBase; diff --git a/modules/simulator/include/mvsim/WorldElements/ElevationMap.h b/modules/simulator/include/mvsim/WorldElements/ElevationMap.h index 42437eb..8e22268 100644 --- a/modules/simulator/include/mvsim/WorldElements/ElevationMap.h +++ b/modules/simulator/include/mvsim/WorldElements/ElevationMap.h @@ -11,8 +11,6 @@ #include #include -#include -#include #include namespace mvsim @@ -52,9 +50,5 @@ class ElevationMap : public WorldElementBase bool debugShowContactPoints_ = false; - private: - // temp vars (declared here to avoid reallocs): - mrpt::tfest::TMatchingPairList corrs_; - mrpt::poses::CPose3D optimalTf_; }; } // namespace mvsim diff --git a/modules/simulator/src/World.cpp b/modules/simulator/src/World.cpp index 4d10985..90d5219 100644 --- a/modules/simulator/src/World.cpp +++ b/modules/simulator/src/World.cpp @@ -272,20 +272,30 @@ void World::internalOnObservation(const Simulable& veh, const mrpt::obs::CObserv arch << *obs; } -std::vector World::getElevationsAt(const mrpt::math::TPoint2Df& worldXY) const +std::set World::getElevationsAt(const mrpt::math::TPoint2Df& worldXY) const { // Assumption: getListOfSimulableObjectsMtx() is already adquired by all possible call paths? - - std::vector ret; + std::set ret; for (const auto& [name, obj] : simulableObjects_) { if (!obj) continue; const auto optZ = obj->getElevationAt(worldXY); - if (optZ) ret.push_back(*optZ); + if (optZ) ret.insert(*optZ); } - if (ret.empty()) ret.push_back(.0); + if (ret.empty()) ret.insert(.0f); return ret; } + +std::optional 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; +} diff --git a/modules/simulator/src/WorldElements/ElevationMap.cpp b/modules/simulator/src/WorldElements/ElevationMap.cpp index 805bc37..6b98f51 100644 --- a/modules/simulator/src/WorldElements/ElevationMap.cpp +++ b/modules/simulator/src/WorldElements/ElevationMap.cpp @@ -9,7 +9,6 @@ #include #include -#include // least-squares methods #include #include #include @@ -178,117 +177,9 @@ void ElevationMap::internalGuiUpdate( void ElevationMap::simul_pre_timestep([[maybe_unused]] const TSimulContext& context) { - // For each vehicle: - // 1) Compute its 3D pose according to the mesh tilt angle. - // 2) Apply gravity force - const double gravity = parent()->get_gravity(); - - const World::VehicleList& lstVehs = this->world_->getListOfVehicles(); - for (auto& nameVeh : lstVehs) - { - world_->getTimeLogger().enter("elevationmap.handle_vehicle"); - - auto& veh = nameVeh.second; - - const size_t nWheels = veh->getNumWheels(); - - // 1) Compute its 3D pose according to the mesh tilt angle. - // Idea: run a least-squares method to find the best - // SE(3) transformation that map the wheels contact point, - // as seen in local & global coordinates. - // (For large tilt angles, may have to run it iteratively...) - // ------------------------------------------------------------- - // the final downwards direction (unit vector (0,0,-1)) as seen in - // vehicle local frame. - mrpt::math::TPoint3D dir_down; - for (int iter = 0; iter < 2; iter++) - { - const mrpt::math::TPose3D& cur_pose = veh->getPose(); - // This object is faster for repeated point projections - const mrpt::poses::CPose3D cur_cpose(cur_pose); - - 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++) - { - const Wheel& wheel = veh->getWheelInfo(iW); - - // Local frame - mrpt::tfest::TMatchingPair corr; - - corr.localIdx = iW; - corr.local = mrpt::math::TPoint3D(wheel.x, wheel.y, 0); - - // Global frame - const mrpt::math::TPoint3D gPt = cur_cpose.composePoint({wheel.x, wheel.y, 0.0}); - auto z = this->getElevationAt(mrpt::math::TPoint2Df(gPt.x, gPt.y)); - if (!z.has_value()) - { - out_of_area = true; - continue; // vehicle is out of bounds! - } - - corr.globalIdx = iW; - corr.global = mrpt::math::TPoint3D(gPt.x, gPt.y, *z); - - corrs_.push_back(corr); - } - if (out_of_area) continue; - - // Register: - double transf_scale; - mrpt::poses::CPose3DQuat tmpl; - - mrpt::tfest::se3_l2(corrs_, tmpl, transf_scale, true /*force scale unity*/); - - 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(); - - veh->setPose(new_pose); - - } // end iters - - // debug contact points: - if (debugShowContactPoints_) - { - gl_debugWheelsContactPoints_->clear(); - for (const auto& c : corrs_) gl_debugWheelsContactPoints_->insertPoint(c.global); - } - - // compute "down" direction: - { - mrpt::poses::CPose3D rot_only; - rot_only.setRotationMatrix(optimalTf_.getRotationMatrix()); - rot_only.inverseComposePoint(.0, .0, -1.0, dir_down.x, dir_down.y, dir_down.z); - } - - // 2) Apply gravity force - // ------------------------------------------------------------- - { - // To chassis: - const double chassis_weight = veh->getChassisMass() * gravity; - const mrpt::math::TPoint2D chassis_com = veh->getChassisCenterOfMass(); - veh->apply_force( - {dir_down.x * chassis_weight, dir_down.y * chassis_weight}, chassis_com); - - // To wheels: - for (size_t iW = 0; iW < nWheels; iW++) - { - const Wheel& wheel = veh->getWheelInfo(iW); - const double wheel_weight = wheel.mass * gravity; - veh->apply_force( - {dir_down.x * wheel_weight, dir_down.y * wheel_weight}, {wheel.x, wheel.y}); - } - } - - world_->getTimeLogger().leave("elevationmap.handle_vehicle"); - } + // Nothing special to do. + // Since Sep-2024, this functionality has moved to + // World::internal_simul_pre_step_terrain_elevation() } void ElevationMap::simul_post_timestep(const TSimulContext& context) diff --git a/modules/simulator/src/World_gui.cpp b/modules/simulator/src/World_gui.cpp index 206b612..3ef4ed0 100644 --- a/modules/simulator/src/World_gui.cpp +++ b/modules/simulator/src/World_gui.cpp @@ -855,7 +855,7 @@ void World::GUI::handle_mouse_operations() { // Find out the "z": get first elevation if many exist. const auto zs = parent_.getElevationsAt(mrpt::math::TPoint2Df(clickedPt.x, clickedPt.y)); - if (!zs.empty()) clickedPt.z = zs[0]; + if (!zs.empty()) clickedPt.z = *zs.begin(); } const auto screen = gui_win->screen(); diff --git a/modules/simulator/src/World_simul.cpp b/modules/simulator/src/World_simul.cpp index b0224e4..4b5e55b 100644 --- a/modules/simulator/src/World_simul.cpp +++ b/modules/simulator/src/World_simul.cpp @@ -9,8 +9,11 @@ #include #include #include +#include #include #include +#include // least-squares methods +#include #include #include @@ -77,15 +80,19 @@ void World::internal_one_timestep(double dt) // 1) Pre-step { - mrpt::system::CTimeLoggerEntry tle(timlogger_, "timestep.0.prestep"); - + mrpt::system::CTimeLoggerEntry tle(timlogger_, "timestep.1.prestep"); for (auto& e : simulableObjects_) if (e.second) e.second->simul_pre_timestep(context); } + // 2) vehicles terrain elevation + { + mrpt::system::CTimeLoggerEntry tle(timlogger_, "timestep.2.terrain_elevation"); + internal_simul_pre_step_terrain_elevation(); + } - // 2) Run dynamics + // 3) Run dynamics { - mrpt::system::CTimeLoggerEntry tle(timlogger_, "timestep.1.dynamics_integrator"); + mrpt::system::CTimeLoggerEntry tle(timlogger_, "timestep.3.dynamics_integrator"); box2d_world_->Step(dt, b2dVelIters_, b2dPosIters_); @@ -94,12 +101,12 @@ void World::internal_one_timestep(double dt) simulTime_ += dt; } - // 3) Save dynamical state and post-step processing: + // 4) Save dynamical state and post-step processing: // This also makes a copy of all objects dynamical state, so service calls // can be answered straight away without waiting for the main simulation // mutex: { - mrpt::system::CTimeLoggerEntry tle(timlogger_, "timestep.3.save_dynstate"); + mrpt::system::CTimeLoggerEntry tle(timlogger_, "timestep.4.save_dynstate"); const auto lckPhys = mrpt::lockHelper(physical_objects_mtx()); const auto lckCopy = mrpt::lockHelper(copy_of_objects_dynstate_mtx_); @@ -128,8 +135,8 @@ void World::internal_one_timestep(double dt) } lckListObjs.unlock(); // for simulableObjects_ - // 4) Wait for 3D sensors (OpenGL raytrace) to get executed on its thread: - mrpt::system::CTimeLoggerEntry tle4(timlogger_, "timestep.4.wait_3D_sensors"); + // 5) Wait for 3D sensors (OpenGL raytrace) to get executed on its thread: + mrpt::system::CTimeLoggerEntry tle4(timlogger_, "timestep.5.wait_3D_sensors"); if (pending_running_sensors_on_3D_scene()) { // Use a huge timeout here to avoid timing out in build farms / cloud @@ -147,8 +154,8 @@ void World::internal_one_timestep(double dt) } tle4.stop(); - // 5) If we have .rawlog generation enabled, process odometry, etc. - mrpt::system::CTimeLoggerEntry tle5(timlogger_, "timestep.5.post_rawlog"); + // 6) If we have .rawlog generation enabled, process odometry, etc. + mrpt::system::CTimeLoggerEntry tle5(timlogger_, "timestep.6.post_rawlog"); internalPostSimulStepForRawlog(); internalPostSimulStepForTrajectory(); @@ -296,3 +303,126 @@ void World::internalPostSimulStepForTrajectory() p.quat().z(), p.quat().w()); } } + +void World::internal_simul_pre_step_terrain_elevation() +{ + // For each vehicle: + // 1) Compute its 3D pose according to the mesh tilt angle. + // 2) Apply gravity force + const double gravity = get_gravity(); + + mrpt::tfest::TMatchingPairList corrs; + mrpt::poses::CPose3D optimalTf; + + const World::VehicleList& lstVehs = getListOfVehicles(); + for (auto& [name, veh] : lstVehs) + { + getTimeLogger().enter("elevationmap.handle_vehicle"); + + const size_t nWheels = veh->getNumWheels(); + + // 1) Compute its 3D pose according to the mesh tilt angle. + // Idea: run a least-squares method to find the best + // SE(3) transformation that map the wheels contact point, + // as seen in local & global coordinates. + // (For large tilt angles, may have to run it iteratively...) + // ------------------------------------------------------------- + // the final downwards direction (unit vector (0,0,-1)) as seen in + // vehicle local frame. + mrpt::math::TPoint3D dir_down; + for (int iter = 0; iter < 2; iter++) + { + const mrpt::math::TPose3D& cur_pose = veh->getPose(); + // This object is faster for repeated point projections + const mrpt::poses::CPose3D cur_cpose(cur_pose); + + 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++) + { + const Wheel& wheel = veh->getWheelInfo(iW); + + // Local frame + mrpt::tfest::TMatchingPair corr; + + corr.localIdx = iW; + corr.local = mrpt::math::TPoint3D(wheel.x, wheel.y, 0); + + // Global frame + const mrpt::math::TPoint3D gPt = cur_cpose.composePoint({wheel.x, wheel.y, 0.0}); + + const mrpt::math::TPoint3D gPtWheelsAxis = + 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! + } + + corr.globalIdx = iW; + corr.global = mrpt::math::TPoint3D(gPt.x, gPt.y, *z); + + corrs.push_back(corr); + } + if (out_of_area) continue; + + // Register: + double transf_scale; + mrpt::poses::CPose3DQuat tmpl; + + mrpt::tfest::se3_l2(corrs, tmpl, transf_scale, true /*force scale unity*/); + + 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(); + + veh->setPose(new_pose); + + } // end iters + +#if 0 + // debug contact points: + if (debugShowContactPoints_) + { + gl_debugWheelsContactPoints_->clear(); + for (const auto& c : corrs_) gl_debugWheelsContactPoints_->insertPoint(c.global); + } +#endif + + // compute "down" direction: + { + mrpt::poses::CPose3D rot_only; + rot_only.setRotationMatrix(optimalTf.getRotationMatrix()); + rot_only.inverseComposePoint(.0, .0, -1.0, dir_down.x, dir_down.y, dir_down.z); + } + + // 2) Apply gravity force + // ------------------------------------------------------------- + { + // To chassis: + const double chassis_weight = veh->getChassisMass() * gravity; + const mrpt::math::TPoint2D chassis_com = veh->getChassisCenterOfMass(); + veh->apply_force( + {dir_down.x * chassis_weight, dir_down.y * chassis_weight}, chassis_com); + + // To wheels: + for (size_t iW = 0; iW < nWheels; iW++) + { + const Wheel& wheel = veh->getWheelInfo(iW); + const double wheel_weight = wheel.mass * gravity; + veh->apply_force( + {dir_down.x * wheel_weight, dir_down.y * wheel_weight}, {wheel.x, wheel.y}); + } + } + + getTimeLogger().leave("elevationmap.handle_vehicle"); + } +}