Add option to generate trajectory2D for backward compatibility

dfki-ric · Nov 26, 2024 · 690d349 · 690d349
1 parent 82115e3
commit 690d349
Showing 1 changed file with 5 additions and 0 deletions.
diff --git a/src/EnvironmentXYZTheta.cpp b/src/EnvironmentXYZTheta.cpp
@@ -1014,6 +1014,11 @@ void EnvironmentXYZTheta::getTrajectory(const vector<int>& stateIDPath,
 #ifdef ENABLE_DEBUG_DRAWINGS
                 V3DD::DRAW_SPHERE("ugv_nav4d_trajectory_poses", pointOnTravPlane, 0.01, V3DD::Color::red);
 #endif
+                //TODO: Only left here until software which still uses trajectory2D is updated to use trajectory3D
+                if (setZToZero){
+                    pointOnTravPlane.z() = 0;
+                }
+
                 Eigen::Vector3d pointOnBody = plan2Body.inverse(Eigen::Isometry) * pointOnTravPlane;
                 if (positions.empty() || !(positions.back().isApprox(pointOnBody)))
                 {