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
*/
public List<BallTracker> getBalls()
{
return balls;
return new ArrayList<>(balls);
}
......
......@@ -19,9 +19,7 @@ import edu.tigers.sumatra.drawable.ShapeMap;
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 @@ public class WorldInfoCollector extends AWorldPredictor
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 @@ 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)
{
if (filterState == null)
......
Markdown is supported
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