Commit d50d1f35 authored by NicolaiO's avatar NicolaiO 🐼 Committed by TIGERs GitLab
Browse files

[#1600] Remove estimation of robot feedback state

Closes #1600

See merge request main/Sumatra!1340

sumatra-commit: 5a82c50e63ade6405ef916c71edf48e26ac7f863
parent e5f55a02
Pipeline #8742 passed with stage
in 3 minutes and 13 seconds
...@@ -489,7 +489,7 @@ public class CamFilter ...@@ -489,7 +489,7 @@ public class CamFilter
*/ */
public List<BallTracker> getBalls() public List<BallTracker> getBalls()
{ {
return balls; return new ArrayList<>(balls);
} }
......
...@@ -19,9 +19,7 @@ import edu.tigers.sumatra.drawable.ShapeMap; ...@@ -19,9 +19,7 @@ import edu.tigers.sumatra.drawable.ShapeMap;
import edu.tigers.sumatra.drawable.ShapeMapSource; import edu.tigers.sumatra.drawable.ShapeMapSource;
import edu.tigers.sumatra.geometry.Geometry; import edu.tigers.sumatra.geometry.Geometry;
import edu.tigers.sumatra.ids.BotID; import edu.tigers.sumatra.ids.BotID;
import edu.tigers.sumatra.math.AngleMath;
import edu.tigers.sumatra.math.pose.Pose; 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.Vector2;
import edu.tigers.sumatra.math.vector.Vector3; import edu.tigers.sumatra.math.vector.Vector3;
import edu.tigers.sumatra.model.SumatraModel; import edu.tigers.sumatra.model.SumatraModel;
...@@ -127,7 +125,7 @@ public class WorldInfoCollector extends AWorldPredictor ...@@ -127,7 +125,7 @@ public class WorldInfoCollector extends AWorldPredictor
private Map<BotID, BotState> getInternalBotStates(final Collection<RobotInfo> robotInfo) private Map<BotID, BotState> getInternalBotStates(final Collection<RobotInfo> robotInfo)
{ {
return robotInfo.stream() return robotInfo.stream()
.map(ri -> ri.getInternalState().map(s -> estimate(s, ri.getTimestamp()))) .map(RobotInfo::getInternalState)
.filter(Optional::isPresent) .filter(Optional::isPresent)
.map(Optional::get) .map(Optional::get)
.collect(Collectors.toMap( .collect(Collectors.toMap(
...@@ -136,16 +134,6 @@ public class WorldInfoCollector extends AWorldPredictor ...@@ -136,16 +134,6 @@ public class WorldInfoCollector extends AWorldPredictor
} }
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) private boolean useInternalState(BotState filterState, BotState internalState)
{ {
if (filterState == null) if (filterState == null)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment