Add backwards motion to alpha ptg

MRPT · Jan 10, 2024 · b5b9560 · b5b9560
1 parent 0c35071
commit b5b9560
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 8 deletions.
diff --git 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
@@ -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
@@ -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()