diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java index 1bdcbfd2..f9da87c6 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/nodes/planners/WayPointFollowerPlanner.java @@ -22,7 +22,7 @@ public class WayPointFollowerPlanner extends PathPlannerNode { private GpsMeasurement target; // we only want to look at the next 10 waypoints as possible candidates - private final int wayPointLOOKAHEADMAX = 50; + private static final int WAY_POINT_LOOKAHEAD_MAX = 50; /** * @param wayPoints the list of waypoints to follow @@ -50,7 +50,7 @@ private int getClosestIndex(ArrayList wayPoints, GPSPoseMessage double min = Double.MAX_VALUE; //note that the brakes will definitely deploy at this int closestIndex = lastClosestIndex; - for (int i = lastClosestIndex; i < (lastClosestIndex + wayPointLOOKAHEADMAX) && i < wayPoints.size(); i++) { + for (int i = lastClosestIndex; i < (lastClosestIndex + WAY_POINT_LOOKAHEAD_MAX) && i < wayPoints.size(); i++) { GPSPoseMessage gpsPoseMessage = wayPoints.get(i).toGpsPoseMessage(0); double d = GPSPoseMessage.getDistance(currentLocation, gpsPoseMessage); if (d < min) { diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java index a504b49c..66ee000e 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/robots/ControllerTesterRobot.java @@ -38,7 +38,7 @@ public static ControllerTesterRobot getInstance() { private ControllerTesterRobot() { super(); - ArrayList wayPoints = null; + ArrayList wayPoints = new ArrayList<>(); try { wayPoints = WayPointUtil.createWayPointsFromWaypointList(RobobuggyConfigFile.getWaypointSourceLogFile()); nodeList.add(new WayPointFollowerPlanner(wayPoints)); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java index eaecea9c..1f9852f3 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/ControllerTester.java @@ -35,9 +35,7 @@ public class ControllerTester extends PeriodicNode { private Matrix x; private double commandedSteeringAngle = 0; private Publisher simulatedPosePub; - private Publisher gpspub; private int simCounter; - private double targetHeading = INITIAL_HEADING_RAD; /** * Create a new {@link PeriodicNode} decorator @@ -63,7 +61,7 @@ public ControllerTester(String name, LocTuple initialPosition) { })); simulatedPosePub = new Publisher(NodeChannel.POSE.getMsgPath()); - gpspub = new Publisher(NodeChannel.GPS.getMsgPath()); + new Publisher(NodeChannel.GPS.getMsgPath()); } @Override diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java index afd9c5c8..5850c4fc 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/FullSimRunner.java @@ -44,7 +44,6 @@ public class FullSimRunner extends BuggyBaseNode { private Publisher gpsPub; private Publisher encPub; private Publisher steerPub; - private Subscriber steerSub; private RobobuggyKFLocalizer localizer; private WayPointFollowerPlanner controller; @@ -84,7 +83,7 @@ public FullSimRunner(String name, LocTuple initialPos) { gpsPub = new Publisher(NodeChannel.GPS.getMsgPath()); encPub = new Publisher(NodeChannel.ENCODER.getMsgPath()); steerPub = new Publisher(NodeChannel.STEERING.getMsgPath()); - steerSub = new Subscriber("Full Sim Toolbox", NodeChannel.DRIVE_CTRL.getMsgPath(), (topicName, m) -> { + new Subscriber("Full Sim Toolbox", NodeChannel.DRIVE_CTRL.getMsgPath(), (topicName, m) -> { updateMotionModel(((DriveControlMessage) m).getAngleDouble()); steerPub.publish(new SteeringMeasurement(Math.toDegrees(((DriveControlMessage) m).getAngleDouble()))); }); diff --git a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java index 3ea917c6..64a6cafa 100644 --- a/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java +++ b/real_time/surface_src/java_src/Alice/src/main/java/com/roboclub/robobuggy/simulation/PlayBackUtil.java @@ -137,6 +137,7 @@ public static Message parseSensorLog(JsonObject sensorDataJson, Gson translator, break; case DriveControlMessage.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, DriveControlMessage.class); + getPrivateInstance().driveCtrlPub.publish(transmitMessage); break; case EncoderMeasurement.VERSION_ID: transmitMessage = translator.fromJson(sensorDataJson, EncoderMeasurement.class);