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;
......@@ -218,7 +218,7 @@ public class ToolBar
observer.onToggleRecord();
}
btnRecSave.setEnabled(true);
});
}, "RecordSaveButton");
t.start();
}
......
/*
* Copyright (c) 2009 - 2020, DHBW Mannheim - TIGERs Mannheim
* Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.sumatra.trajectory;
......@@ -14,6 +14,7 @@ import java.util.function.Function;
public final class BangBangTrajectoryFactory
{
private static final double MAX_VEL_TOLERANCE = 0.2;
private static final float SYNC_ACCURACY = 1e-3f;
......@@ -54,8 +55,14 @@ public final class BangBangTrajectoryFactory
final double acc
)
{
return new BangBangTrajectory2D()
.generate(s0, s1, v0, (float) vmax, (float) acc, SYNC_ACCURACY, Function.identity());
return new BangBangTrajectory2D().generate(
s0,
s1,
adaptVel(v0, vmax),
(float) vmax,
(float) acc,
SYNC_ACCURACY,
Function.identity());
}
......@@ -67,8 +74,12 @@ public final class BangBangTrajectoryFactory
final double maxAcc
)
{
return new BangBangTrajectory1D()
.generate((float) initialPos, (float) finalPos, (float) initialVel, (float) maxVel, (float) maxAcc);
return new BangBangTrajectory1D().generate(
(float) initialPos,
(float) finalPos,
(float) adaptVel(initialVel, maxVel),
(float) maxVel,
(float) maxAcc);
}
......@@ -81,6 +92,38 @@ public final class BangBangTrajectoryFactory
)
{
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;
import edu.tigers.sumatra.math.vector.Vector2;
import edu.tigers.sumatra.math.vector.Vector2f;
import edu.tigers.sumatra.model.SumatraModel;
import lombok.Getter;
import lombok.extern.log4j.Log4j2;
import java.util.ArrayList;
......@@ -168,6 +169,12 @@ public class Geometry
comment = "Delay [s] from giving a robot command to receiving the reaction on this command from vision")
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 fieldWBorders;
......
/*
* Copyright (c) 2009 - 2019, DHBW Mannheim - TIGERs Mannheim
* Copyright (c) 2009 - 2021, DHBW Mannheim - TIGERs Mannheim
*/
package edu.tigers.sumatra.persistence;
......@@ -124,8 +124,6 @@ public class BerkeleyAsyncRecorder
private final ScheduledExecutorService execService;
/**
*/
RecordSaver()
{
execService = Executors.newSingleThreadScheduledExecutor(new NamedThreadFactory("RecordSaver"));
......@@ -160,6 +158,17 @@ public class BerkeleyAsyncRecorder
execService.execute(this::printPeriod);
execService.execute(db::close);
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
{
// but it is on a different camera already, copy its state from there
tracker = new RobotTracker(robot, filteredBot.get());
robots.put(robot.getBotId(), tracker);
} else
{
// completely new robot on the field
tracker = new RobotTracker(robot);
robots.put(robot.getBotId(), tracker);
}
robots.put(robot.getBotId(), tracker);
}
......@@ -446,7 +445,7 @@ public class CamFilter
// 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 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 :)
BallTracker tracker;
......@@ -503,7 +502,7 @@ public class CamFilter
{
List<IDrawableShape> shapes = new ArrayList<>();
if (!calibration.isPresent())
if (calibration.isEmpty())
{
return shapes;
}
......@@ -604,7 +603,8 @@ public class CamFilter
{
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);
pos.setFill(false);
......
......@@ -187,7 +187,9 @@ public class WorldInfoCollector extends AWorldPredictor
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);
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;
......@@ -10,8 +10,6 @@ import edu.tigers.sumatra.bot.RobotInfo;
import edu.tigers.sumatra.geometry.Geometry;
import edu.tigers.sumatra.ids.BotID;
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.vector.IVector2;
import edu.tigers.sumatra.wp.data.BallContact;
......@@ -82,9 +80,7 @@ public class BallContactCalculator
final IVector2 optimalBallPossPos = BotShape.getKickerCenterPos(pose.getPos(), pose.getOrientation(),
center2Dribbler + Geometry.getBallRadius());
ICircle circle = Circle.createCircle(optimalBallPossPos, ballPossTolerance);
return circle.isPointInShape(ballPos);
return optimalBallPossPos.distanceTo(ballPos) < ballPossTolerance;
}
......
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