From b5b95601afadb6591a8c4f6322632f3a4f5e8399 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Wed, 10 Jan 2024 10:59:36 +0100 Subject: [PATCH] Add backwards motion to alpha ptg --- doc/source/doxygen-docs/changelog.md | 2 ++ .../mrpt/nav/tpspace/CPTG_DiffDrive_alpha.h | 2 +- libs/nav/src/tpspace/CPTG_DiffDrive_alpha.cpp | 21 ++++++++++++------- 3 files changed, 17 insertions(+), 8 deletions(-) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index df524b83cc..f878b116ed 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -4,6 +4,8 @@ - Changes in libraries: - \ref mrpt_obs_grp - mrpt::obs::CObservation::load() is now protected with a std::mutex for safe multi-threading usage. + - \ref mrpt_nav_grp + - mrpt::nav::CPTG_DiffDrive_alpha now has a "K" parameter for generating backwards trajectories too. - BUG FIXES: - Fix wrong filenames in `rawlog-edit --externalize` when sensor labels contain the `/` character (e.g. mimicking ROS topic names). - Fix crash in mrpt::ros2bridge::toROS() for XYZIRT point clouds. diff --git a/libs/nav/include/mrpt/nav/tpspace/CPTG_DiffDrive_alpha.h b/libs/nav/include/mrpt/nav/tpspace/CPTG_DiffDrive_alpha.h index bd5eaa42bf..776203669f 100644 --- a/libs/nav/include/mrpt/nav/tpspace/CPTG_DiffDrive_alpha.h +++ b/libs/nav/include/mrpt/nav/tpspace/CPTG_DiffDrive_alpha.h @@ -56,6 +56,6 @@ class CPTG_DiffDrive_alpha : public CPTG_DiffDrive_CollisionGridBased void loadDefaultParams() override; protected: - double cte_a0v{0}, cte_a0w{0}; + double cte_a0v = 0, cte_a0w = 0, K = 1.0; }; } // namespace mrpt::nav diff --git a/libs/nav/src/tpspace/CPTG_DiffDrive_alpha.cpp b/libs/nav/src/tpspace/CPTG_DiffDrive_alpha.cpp index 3e451079ed..a8981a975a 100644 --- a/libs/nav/src/tpspace/CPTG_DiffDrive_alpha.cpp +++ b/libs/nav/src/tpspace/CPTG_DiffDrive_alpha.cpp @@ -29,6 +29,7 @@ void CPTG_DiffDrive_alpha::loadFromConfigFile( cte_a0v_deg, double, cte_a0v, cfg, sSection); MRPT_LOAD_HERE_CONFIG_VAR_DEGREES_NO_DEFAULT( cte_a0w_deg, double, cte_a0w, cfg, sSection); + MRPT_LOAD_HERE_CONFIG_VAR(K, double, K, cfg, sSection); } void CPTG_DiffDrive_alpha::saveToConfigFile( mrpt::config::CConfigFileBase& cfg, const std::string& sSection) const @@ -43,6 +44,7 @@ void CPTG_DiffDrive_alpha::saveToConfigFile( cfg.write( sSection, "cte_a0w_deg", mrpt::RAD2DEG(cte_a0v), WN, WV, "Contant for omega profile [deg]."); + cfg.write(sSection, "K", K, WN, WV, "1: forward, -1: backwards"); MRPT_END } @@ -51,8 +53,8 @@ std::string CPTG_DiffDrive_alpha::getDescription() const { char str[100]; os::sprintf( - str, 100, "CPTG_DiffDrive_alpha,av=%udeg,aw=%udeg", - (int)RAD2DEG(cte_a0v), (int)RAD2DEG(cte_a0w)); + str, 100, "CPTG_DiffDrive_alpha,av=%udeg,aw=%udeg,K=%i", + (int)RAD2DEG(cte_a0v), (int)RAD2DEG(cte_a0w), (int)K); return std::string(str); } @@ -63,16 +65,21 @@ void CPTG_DiffDrive_alpha::serializeFrom( switch (version) { - case 0: in >> cte_a0v >> cte_a0w; break; + case 0: + case 1: + in >> cte_a0v >> cte_a0w; + if (version >= 1) in >> K; + break; + default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); }; } -uint8_t CPTG_DiffDrive_alpha::serializeGetVersion() const { return 0; } +uint8_t CPTG_DiffDrive_alpha::serializeGetVersion() const { return 1; } void CPTG_DiffDrive_alpha::serializeTo(mrpt::serialization::CArchive& out) const { CPTG_DiffDrive_CollisionGridBased::internal_writeToStream(out); - out << cte_a0v << cte_a0w; + out << cte_a0v << cte_a0w << K; } /*--------------------------------------------------------------- ptgDiffDriveSteeringFunction @@ -88,8 +95,8 @@ void CPTG_DiffDrive_alpha::ptgDiffDriveSteeringFunction( while (At_a < -M_PI) At_a += (float)M_2PI; - v = V_MAX * exp(-square(At_a / cte_a0v)); - w = W_MAX * (-0.5f + (1 / (1 + exp(-At_a / cte_a0w)))); + v = sign(K) * V_MAX * exp(-square(At_a / cte_a0v)); + w = sign(K) * W_MAX * (-0.5f + (1 / (1 + exp(-At_a / cte_a0w)))); } void CPTG_DiffDrive_alpha::loadDefaultParams()