Add option to generate trajectory2D for backward compatibility

Author: haider8645

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)))
                 {