Commit 6a983834 authored by MarkG's avatar MarkG Committed by TIGERs GitLab
Browse files

Resolve "Improve robot movement in external simulators"

Closes #1586

See merge request main/Sumatra!1332

sumatra-commit: 507b9394234ca785157a11b42c15c6c197360fdf
parent 9c4b48d8
Pipeline #8653 passed with stage
in 2 minutes and 55 seconds
/* /*
* Copyright (c) 2009 - 2020, DHBW Mannheim - TIGERs Mannheim * Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
*/ */
package edu.tigers.sumatra.view.toolbar; package edu.tigers.sumatra.view.toolbar;
...@@ -218,7 +218,7 @@ public class ToolBar ...@@ -218,7 +218,7 @@ public class ToolBar
observer.onToggleRecord(); observer.onToggleRecord();
} }
btnRecSave.setEnabled(true); btnRecSave.setEnabled(true);
}); }, "RecordSaveButton");
t.start(); t.start();
} }
......
/* /*
* Copyright (c) 2009 - 2020, DHBW Mannheim - TIGERs Mannheim * Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
*/ */
package edu.tigers.sumatra.trajectory; package edu.tigers.sumatra.trajectory;
...@@ -14,6 +14,7 @@ import java.util.function.Function; ...@@ -14,6 +14,7 @@ import java.util.function.Function;
public final class BangBangTrajectoryFactory public final class BangBangTrajectoryFactory
{ {
private static final double MAX_VEL_TOLERANCE = 0.2;
private static final float SYNC_ACCURACY = 1e-3f; private static final float SYNC_ACCURACY = 1e-3f;
...@@ -54,8 +55,14 @@ public final class BangBangTrajectoryFactory ...@@ -54,8 +55,14 @@ public final class BangBangTrajectoryFactory
final double acc final double acc
) )
{ {
return new BangBangTrajectory2D() return new BangBangTrajectory2D().generate(
.generate(s0, s1, v0, (float) vmax, (float) acc, SYNC_ACCURACY, Function.identity()); s0,
s1,
adaptVel(v0, vmax),
(float) vmax,
(float) acc,
SYNC_ACCURACY,
Function.identity());
} }
...@@ -67,8 +74,12 @@ public final class BangBangTrajectoryFactory ...@@ -67,8 +74,12 @@ public final class BangBangTrajectoryFactory
final double maxAcc final double maxAcc
) )
{ {
return new BangBangTrajectory1D() return new BangBangTrajectory1D().generate(
.generate((float) initialPos, (float) finalPos, (float) initialVel, (float) maxVel, (float) maxAcc); (float) initialPos,
(float) finalPos,
(float) adaptVel(initialVel, maxVel),
(float) maxVel,
(float) maxAcc);
} }
...@@ -81,6 +92,38 @@ public final class BangBangTrajectoryFactory ...@@ -81,6 +92,38 @@ public final class BangBangTrajectoryFactory
) )
{ {
var adaptedFinalPos = initialPos + AngleMath.normalizeAngle(finalPos - AngleMath.normalizeAngle(initialPos)); var adaptedFinalPos = initialPos + AngleMath.normalizeAngle(finalPos - AngleMath.normalizeAngle(initialPos));
return new BangBangTrajectory1DOrient(singleDim(initialPos, adaptedFinalPos, initialVel, maxVel, maxAcc)); return new BangBangTrajectory1DOrient(
singleDim(initialPos, adaptedFinalPos, adaptVel(initialVel, maxVel), maxVel, maxAcc));
}
/**
* Limit the current speed to the max speed, if it is only slightly above velMax.
* Trajectories are often generated based on the last trajectory and imprecision will lead to a
* propagating error that lets the robot drive significantly faster than velMax (like >+0.2m/s with 1.5m/s)
*
* @param v0
* @param vMax
* @return
*/
private static IVector2 adaptVel(IVector2 v0, double vMax)
{
var curVelAbs = v0.getLength2();
if (curVelAbs > vMax && curVelAbs < vMax + MAX_VEL_TOLERANCE)
{
return v0.scaleToNew(vMax);
}
return v0;
}
private static double adaptVel(double v0, double vMax)
{
var curVelAbs = v0;
if (curVelAbs > vMax && curVelAbs < vMax + MAX_VEL_TOLERANCE)
{
return vMax;
}
return v0;
} }
} }
...@@ -25,6 +25,7 @@ import edu.tigers.sumatra.math.vector.IVector2; ...@@ -25,6 +25,7 @@ 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.Vector2f; import edu.tigers.sumatra.math.vector.Vector2f;
import edu.tigers.sumatra.model.SumatraModel; import edu.tigers.sumatra.model.SumatraModel;
import lombok.Getter;
import lombok.extern.log4j.Log4j2; import lombok.extern.log4j.Log4j2;
import java.util.ArrayList; import java.util.ArrayList;
...@@ -168,6 +169,12 @@ public class Geometry ...@@ -168,6 +169,12 @@ public class Geometry
comment = "Delay [s] from giving a robot command to receiving the reaction on this command from vision") comment = "Delay [s] from giving a robot command to receiving the reaction on this command from vision")
private static double feedbackDelay = 0.06; private static double feedbackDelay = 0.06;
@Getter
@Configurable(spezis = { "NICOLAI", "SUMATRA", "LAB", "TISCH", "ROBOCUP", "ANDRE", "SIMULATOR" },
defValueSpezis = { "0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "0.0125" },
comment = "Delay [s] between path planning trajectory state and robot feedback state")
private static double robotFeedbackDelay = 0.0;
private final IRectangle field; private final IRectangle field;
private final IRectangle fieldWBorders; private final IRectangle fieldWBorders;
......
/* /*
* Copyright (c) 2009 - 2019, DHBW Mannheim - TIGERs Mannheim * Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
*/ */
package edu.tigers.sumatra.persistence; package edu.tigers.sumatra.persistence;
...@@ -124,8 +124,6 @@ public class BerkeleyAsyncRecorder ...@@ -124,8 +124,6 @@ public class BerkeleyAsyncRecorder
private final ScheduledExecutorService execService; private final ScheduledExecutorService execService;
/**
*/
RecordSaver() RecordSaver()
{ {
execService = Executors.newSingleThreadScheduledExecutor(new NamedThreadFactory("RecordSaver")); execService = Executors.newSingleThreadScheduledExecutor(new NamedThreadFactory("RecordSaver"));
...@@ -160,6 +158,17 @@ public class BerkeleyAsyncRecorder ...@@ -160,6 +158,17 @@ public class BerkeleyAsyncRecorder
execService.execute(this::printPeriod); execService.execute(this::printPeriod);
execService.execute(db::close); execService.execute(db::close);
execService.shutdown(); execService.shutdown();
try
{
boolean terminated = execService.awaitTermination(10, TimeUnit.SECONDS);
if (!terminated)
{
log.warn("Could not terminate record saver within 10s");
}
} catch (InterruptedException e)
{
Thread.currentThread().interrupt();
}
} }
} }
} }
...@@ -378,13 +378,12 @@ public class CamFilter ...@@ -378,13 +378,12 @@ public class CamFilter
{ {
// but it is on a different camera already, copy its state from there // but it is on a different camera already, copy its state from there
tracker = new RobotTracker(robot, filteredBot.get()); tracker = new RobotTracker(robot, filteredBot.get());
robots.put(robot.getBotId(), tracker);
} else } else
{ {
// completely new robot on the field // completely new robot on the field
tracker = new RobotTracker(robot); tracker = new RobotTracker(robot);
robots.put(robot.getBotId(), tracker);
} }
robots.put(robot.getBotId(), tracker);
} }
...@@ -446,7 +445,7 @@ public class CamFilter ...@@ -446,7 +445,7 @@ public class CamFilter
// if this ball is not used by any other tracker we may do: // if this ball is not used by any other tracker we may do:
// - if we know the field size => only accept new balls on the field (not in boundary area) // - if we know the field size => only accept new balls on the field (not in boundary area)
// - if we don't know the field size => simply accept the ball // - if we don't know the field size => simply accept the ball
if (!fieldRectWithBoundary.isPresent() || fieldRectWithBoundary.get().isPointInShape(cam.getPos().getXYVector())) if (fieldRectWithBoundary.isEmpty() || fieldRectWithBoundary.get().isPointInShape(cam.getPos().getXYVector()))
{ {
// if nobody else wanted this ball we create a new tracker, very gentle :) // if nobody else wanted this ball we create a new tracker, very gentle :)
BallTracker tracker; BallTracker tracker;
...@@ -503,7 +502,7 @@ public class CamFilter ...@@ -503,7 +502,7 @@ public class CamFilter
{ {
List<IDrawableShape> shapes = new ArrayList<>(); List<IDrawableShape> shapes = new ArrayList<>();
if (!calibration.isPresent()) if (calibration.isEmpty())
{ {
return shapes; return shapes;
} }
...@@ -604,7 +603,8 @@ public class CamFilter ...@@ -604,7 +603,8 @@ public class CamFilter
{ {
List<IDrawableShape> shapes = new ArrayList<>(); List<IDrawableShape> shapes = new ArrayList<>();
for (CamBall b : ballHistory) var ballHistorySnapshot = new ArrayList<>(ballHistory);
for (CamBall b : ballHistorySnapshot)
{ {
DrawableCircle pos = new DrawableCircle(b.getFlatPos(), 15, Color.BLACK); DrawableCircle pos = new DrawableCircle(b.getFlatPos(), 15, Color.BLACK);
pos.setFill(false); pos.setFill(false);
......
...@@ -187,7 +187,9 @@ public class WorldInfoCollector extends AWorldPredictor ...@@ -187,7 +187,9 @@ public class WorldInfoCollector extends AWorldPredictor
robotInfo.getTrajectory().ifPresentOrElse(t -> sync.add(t, lastWFTimestamp), sync::reset); robotInfo.getTrajectory().ifPresentOrElse(t -> sync.add(t, lastWFTimestamp), sync::reset);
} }
var feedbackDelay = useInternalState ? 0.0 : Geometry.getFeedbackDelay(); var feedbackDelay = useInternalState
? Geometry.getRobotFeedbackDelay()
: Geometry.getFeedbackDelay();
var trackedState = sync.updateState(lastWFTimestamp, feedbackDelay, currentBotState); var trackedState = sync.updateState(lastWFTimestamp, feedbackDelay, currentBotState);
Optional<State> bufferedState = sync.getLatestState(); Optional<State> bufferedState = sync.getLatestState();
......
/* /*
* Copyright (c) 2009 - 2020, DHBW Mannheim - TIGERs Mannheim * Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
*/ */
package edu.tigers.sumatra.wp.util; package edu.tigers.sumatra.wp.util;
...@@ -10,8 +10,6 @@ import edu.tigers.sumatra.bot.RobotInfo; ...@@ -10,8 +10,6 @@ import edu.tigers.sumatra.bot.RobotInfo;
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.botshape.BotShape; import edu.tigers.sumatra.math.botshape.BotShape;
import edu.tigers.sumatra.math.circle.Circle;
import edu.tigers.sumatra.math.circle.ICircle;
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.IVector2;
import edu.tigers.sumatra.wp.data.BallContact; import edu.tigers.sumatra.wp.data.BallContact;
...@@ -82,9 +80,7 @@ public class BallContactCalculator ...@@ -82,9 +80,7 @@ public class BallContactCalculator
final IVector2 optimalBallPossPos = BotShape.getKickerCenterPos(pose.getPos(), pose.getOrientation(), final IVector2 optimalBallPossPos = BotShape.getKickerCenterPos(pose.getPos(), pose.getOrientation(),
center2Dribbler + Geometry.getBallRadius()); center2Dribbler + Geometry.getBallRadius());
ICircle circle = Circle.createCircle(optimalBallPossPos, ballPossTolerance); return optimalBallPossPos.distanceTo(ballPos) < ballPossTolerance;
return circle.isPointInShape(ballPos);
} }
......
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