Use correct start point

haider8645 · haider8645 · commit 19e3ee8003f6 · 2024-11-25T15:07:34.000+01:00
diff --git a/src/EnvironmentXYZTheta.cpp b/src/EnvironmentXYZTheta.cpp
@@ -1016,7 +1016,7 @@ void EnvironmentXYZTheta::getTrajectory(const vector<int>& stateIDPath,
             for (const base::Pose2D &p : cwp.poses)
             {
                 Eigen::Vector3d point{p.position.x(), p.position.y(), 0};
-                Eigen::Vector3d globalPoint = point + Eigen::Vector3d(start.x(), start.y(), posWorld.z());
+                Eigen::Vector3d globalPoint = point + Eigen::Vector3d(start.x(), start.y(), start.z());
                 Eigen::Vector3d pointP = travNodePlane.projection(globalPoint); // Offset by posWorld
 
 #ifdef ENABLE_DEBUG_DRAWINGS

Original file line number	Diff line number	Diff line change
`@@ -1016,7 +1016,7 @@ void EnvironmentXYZTheta::getTrajectory(const vector<int>& stateIDPath,`
`1016`	`1016`	`for (const base::Pose2D &p : cwp.poses)`
`1017`	`1017`	`{`
`1018`	`1018`	`Eigen::Vector3d point{p.position.x(), p.position.y(), 0};`
`1019`		`- Eigen::Vector3d globalPoint = point + Eigen::Vector3d(start.x(), start.y(), posWorld.z());`
	`1019`	`+ Eigen::Vector3d globalPoint = point + Eigen::Vector3d(start.x(), start.y(), start.z());`
`1020`	`1020`	`Eigen::Vector3d pointP = travNodePlane.projection(globalPoint); // Offset by posWorld`
`1021`	`1021`
`1022`	`1022`	`#ifdef ENABLE_DEBUG_DRAWINGS`