[#1600] Remove estimation of robot feedback state

Closes #1600 See merge request main/Sumatra!1340 sumatra-commit: 5a82c50e63ade6405ef916c71edf48e26ac7f863
ZJU-RoboCup · Mar 30, 2021 · d50d1f3 · d50d1f3
1 parent e5f55a0
commit d50d1f3
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 14 deletions.
diff --git a/modules/moduli-vision/src/main/java/edu/tigers/sumatra/vision/CamFilter.java b/modules/moduli-vision/src/main/java/edu/tigers/sumatra/vision/CamFilter.java
@@ -489,7 +489,7 @@ public int getCamId()
 	 */
 	public List<BallTracker> getBalls()
 	{
-		return balls;
+		return new ArrayList<>(balls);
 	}
 
 

diff --git a/modules/moduli-wp/src/main/java/edu/tigers/sumatra/wp/WorldInfoCollector.java b/modules/moduli-wp/src/main/java/edu/tigers/sumatra/wp/WorldInfoCollector.java
@@ -19,9 +19,7 @@
 import edu.tigers.sumatra.drawable.ShapeMapSource;
 import edu.tigers.sumatra.geometry.Geometry;
 import edu.tigers.sumatra.ids.BotID;
-import edu.tigers.sumatra.math.AngleMath;
 import edu.tigers.sumatra.math.pose.Pose;
-import edu.tigers.sumatra.math.vector.IVector2;
 import edu.tigers.sumatra.math.vector.Vector2;
 import edu.tigers.sumatra.math.vector.Vector3;
 import edu.tigers.sumatra.model.SumatraModel;
@@ -127,7 +125,7 @@ private Map<BotID, FilteredVisionBot> getFilteredBots(final Collection<FilteredV
 	private Map<BotID, BotState> getInternalBotStates(final Collection<RobotInfo> robotInfo)
 	{
 		return robotInfo.stream()
-				.map(ri -> ri.getInternalState().map(s -> estimate(s, ri.getTimestamp())))
+				.map(RobotInfo::getInternalState)
 				.filter(Optional::isPresent)
 				.map(Optional::get)
 				.collect(Collectors.toMap(
@@ -136,16 +134,6 @@ private Map<BotID, BotState> getInternalBotStates(final Collection<RobotInfo> ro
 	}
 
 
-	private BotState estimate(BotState state, long timestamp)
-	{
-		double dt = (lastWFTimestamp - timestamp) / 1e9;
-		IVector2 pos = state.getPos().addNew(state.getVel2().multiplyNew(1000 * dt));
-		double orientation = AngleMath.normalizeAngle(state.getOrientation() + state.getAngularVel() * dt);
-		Pose pose = Pose.from(pos, orientation);
-		return BotState.of(state.getBotID(), State.of(pose, state.getVel3()));
-	}
-
-
 	private boolean useInternalState(BotState filterState, BotState internalState)
 	{
 		if (filterState == null)
-Original file line number
+Diff line change
@@ Expand Up / @@ -489,7 +489,7 @@ public int getCamId() @@
     	 */
     	public List<BallTracker> getBalls()
     	{
-    		return balls;
+    		return new ArrayList<>(balls);
     	}
@@ Expand Down @@