-
Notifications
You must be signed in to change notification settings - Fork 25
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #714 from idsc-frazzoli/dh347
dh347
- Loading branch information
Showing
40 changed files
with
520 additions
and
153 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
47 changes: 47 additions & 0 deletions
47
src/main/java/ch/ethz/idsc/demo/jph/lidar/local/LogPosePostProvider.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,47 @@ | ||
| // code by jph | ||
| package ch.ethz.idsc.demo.jph.lidar.local; | ||
|
|
||
| import java.io.File; | ||
| import java.util.List; | ||
| import java.util.stream.Collectors; | ||
| import java.util.stream.Stream; | ||
|
|
||
| import ch.ethz.idsc.gokart.core.pos.GokartPoseEvent; | ||
| import ch.ethz.idsc.gokart.core.pos.GokartPoseListener; | ||
| import ch.ethz.idsc.gokart.core.pos.LocalizationConfig; | ||
| import ch.ethz.idsc.gokart.offline.api.GokartLogAdapter; | ||
| import ch.ethz.idsc.gokart.offline.api.GokartLogInterface; | ||
| import ch.ethz.idsc.gokart.offline.pose.LogPosePostInject; | ||
| import ch.ethz.idsc.tensor.Scalar; | ||
|
|
||
| public class LogPosePostProvider { | ||
| public static void main(String[] args) throws Exception { | ||
| File root = new File("/media/datahaki/data/gokart/cuts/20190311"); | ||
| File dest = new File("/media/datahaki/data/gokart/localization/20190311"); | ||
| dest.mkdir(); | ||
| List<File> list = Stream.of(root.listFiles()) // | ||
| .filter(File::isDirectory) // | ||
| .sorted() // | ||
| .skip(1) // | ||
| .limit(1) // | ||
| .collect(Collectors.toList()); | ||
| for (File folder : list) { | ||
| System.out.println(folder.getName()); | ||
| GokartLogInterface gokartLogInterface = GokartLogAdapter.of(folder, "log.lcm"); | ||
| LidarLocalizationTable lidarLocalizationTable = new LidarLocalizationTable(); | ||
| lidarLocalizationTable.listeners.add(new GokartPoseListener() { | ||
| @Override | ||
| public void getEvent(GokartPoseEvent gokartPoseEvent) { | ||
| Scalar quality = gokartPoseEvent.getQuality(); | ||
| if (!LocalizationConfig.GLOBAL.isQualityOk(quality)) | ||
| System.err.println("quality! " + quality); | ||
| } | ||
| }); | ||
| lidarLocalizationTable.lidarLocalizationCore.resetPose(gokartLogInterface.pose()); | ||
| // --- | ||
| LogPosePostInject logPosePostInject = new LogPosePostInject(); | ||
| lidarLocalizationTable.listeners.add(logPosePostInject); | ||
| logPosePostInject.process(gokartLogInterface.file(), new File(folder, "post.lcm"), lidarLocalizationTable); | ||
| } | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
46 changes: 46 additions & 0 deletions
46
src/main/java/ch/ethz/idsc/demo/jph/video/DriftLinesRender.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,46 @@ | ||
| // code by jph | ||
| package ch.ethz.idsc.demo.jph.video; | ||
|
|
||
| import java.awt.Color; | ||
| import java.awt.Graphics2D; | ||
|
|
||
| import ch.ethz.idsc.gokart.core.pos.GokartPoseEvent; | ||
| import ch.ethz.idsc.gokart.core.pos.GokartPoseHelper; | ||
| import ch.ethz.idsc.gokart.core.pos.GokartPoseListener; | ||
| import ch.ethz.idsc.owl.data.BoundedLinkedList; | ||
| import ch.ethz.idsc.owl.gui.RenderInterface; | ||
| import ch.ethz.idsc.owl.gui.win.GeometricLayer; | ||
| import ch.ethz.idsc.tensor.Tensor; | ||
| import ch.ethz.idsc.tensor.Tensors; | ||
|
|
||
| public class DriftLinesRender implements GokartPoseListener, RenderInterface { | ||
| private static final Color COLOR = new Color(128, 128, 128, 64); | ||
| private static final Tensor PATH = Tensors.of( // | ||
| Tensors.vector(0.0, 0), // | ||
| Tensors.vector(0.4, 0)); | ||
| // --- | ||
| private final BoundedLinkedList<Tensor> boundedLinkedList; | ||
|
|
||
| public DriftLinesRender(int limit) { | ||
| boundedLinkedList = new BoundedLinkedList<>(limit); | ||
| } | ||
|
|
||
| @Override // from GokartPoseListener | ||
| public void getEvent(GokartPoseEvent gokartPoseEvent) { | ||
| synchronized (boundedLinkedList) { | ||
| boundedLinkedList.add(GokartPoseHelper.toSE2Matrix(gokartPoseEvent.getPose())); | ||
| } | ||
| } | ||
|
|
||
| @Override // from RenderInterface | ||
| public void render(GeometricLayer geometricLayer, Graphics2D graphics) { | ||
| synchronized (boundedLinkedList) { | ||
| graphics.setColor(COLOR); | ||
| for (Tensor matrix : boundedLinkedList) { | ||
| geometricLayer.pushMatrix(matrix); | ||
| graphics.draw(geometricLayer.toPath2D(PATH)); | ||
| geometricLayer.popMatrix(); | ||
| } | ||
| } | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
126 changes: 126 additions & 0 deletions
126
src/main/java/ch/ethz/idsc/demo/jph/video/OfflineRender.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,126 @@ | ||
| // code by jph | ||
| package ch.ethz.idsc.demo.jph.video; | ||
|
|
||
| import java.awt.Color; | ||
| import java.awt.Font; | ||
| import java.awt.Graphics2D; | ||
| import java.nio.ByteBuffer; | ||
| import java.util.Objects; | ||
|
|
||
| import ch.ethz.idsc.gokart.core.mpc.ControlAndPredictionSteps; | ||
| import ch.ethz.idsc.gokart.core.mpc.ControlAndPredictionStepsMessage; | ||
| import ch.ethz.idsc.gokart.core.pos.GokartPoseEvent; | ||
| import ch.ethz.idsc.gokart.dev.linmot.LinmotGetEvent; | ||
| import ch.ethz.idsc.gokart.dev.rimo.RimoGetEvent; | ||
| import ch.ethz.idsc.gokart.dev.rimo.RimoPutEvent; | ||
| import ch.ethz.idsc.gokart.dev.rimo.RimoPutHelper; | ||
| import ch.ethz.idsc.gokart.gui.GokartLcmChannel; | ||
| import ch.ethz.idsc.gokart.gui.GokartStatusEvent; | ||
| import ch.ethz.idsc.gokart.gui.top.AccelerationRender; | ||
| import ch.ethz.idsc.gokart.gui.top.ExtrudedFootprintRender; | ||
| import ch.ethz.idsc.gokart.gui.top.GokartRender; | ||
| import ch.ethz.idsc.gokart.gui.top.GroundSpeedRender; | ||
| import ch.ethz.idsc.gokart.gui.top.MPCPredictionRender; | ||
| import ch.ethz.idsc.gokart.gui.top.MPCPredictionSequenceRender; | ||
| import ch.ethz.idsc.gokart.gui.top.TachometerMustangDash; | ||
| import ch.ethz.idsc.gokart.lcm.OfflineLogListener; | ||
| import ch.ethz.idsc.gokart.lcm.autobox.LinmotLcmServer; | ||
| import ch.ethz.idsc.gokart.lcm.autobox.RimoLcmServer; | ||
| import ch.ethz.idsc.gokart.offline.channel.Vmu931ImuChannel; | ||
| import ch.ethz.idsc.owl.gui.RenderInterface; | ||
| import ch.ethz.idsc.owl.gui.win.GeometricLayer; | ||
| import ch.ethz.idsc.retina.imu.vmu931.Vmu931ImuFrame; | ||
| import ch.ethz.idsc.sophus.group.Se2Utils; | ||
| import ch.ethz.idsc.tensor.Scalar; | ||
| import ch.ethz.idsc.tensor.Tensor; | ||
| import ch.ethz.idsc.tensor.Tensors; | ||
| import ch.ethz.idsc.tensor.mat.DiagonalMatrix; | ||
| import ch.ethz.idsc.tensor.mat.Inverse; | ||
| import ch.ethz.idsc.tensor.sca.Round; | ||
|
|
||
| public class OfflineRender implements OfflineLogListener, RenderInterface { | ||
| private final MPCPredictionSequenceRender mpcPredictionSequenceRender = new MPCPredictionSequenceRender(20); | ||
| private final MPCPredictionRender mpcPredictionRender = new MPCPredictionRender(); | ||
| private final DriftLinesRender driftLinesRender = new DriftLinesRender(100); | ||
| private final GokartRender gokartRender = new GokartRender(); | ||
| private final AccelerationRender accelerationRender; | ||
| private final GroundSpeedRender groundSpeedRender; | ||
| private final TachometerMustangDash tachometerMustangDash; | ||
| private final ExtrudedFootprintRender extrudedFootprintRender = new ExtrudedFootprintRender(); | ||
| private final String poseChannel; | ||
| // --- | ||
| private LinmotGetEvent linmotGetEvent; | ||
|
|
||
| public OfflineRender(Tensor model2pixel, String poseChannel) { | ||
| this.poseChannel = poseChannel; | ||
| accelerationRender = new AccelerationRender(50, // | ||
| Inverse.of(model2pixel) // | ||
| .dot(Se2Utils.toSE2Matrix(Tensors.vector(960 + 250, 140, 0))) // | ||
| .dot(Se2Utils.toSE2Matrix(Tensors.vector(0, 0, -Math.PI / 2))) // | ||
| .dot(DiagonalMatrix.of(8, -8, 1))); | ||
| Tensor matrix = Inverse.of(model2pixel) // | ||
| .dot(Se2Utils.toSE2Matrix(Tensors.vector(960 - 250, 140, 0))) // | ||
| .dot(Se2Utils.toSE2Matrix(Tensors.vector(0, 0, -Math.PI / 2))) // | ||
| .dot(DiagonalMatrix.of(10, -10, 1)); | ||
| groundSpeedRender = new GroundSpeedRender(50, matrix); | ||
| tachometerMustangDash = new TachometerMustangDash(matrix); // | ||
| } | ||
|
|
||
| @Override // from OfflineLogListener | ||
| public void event(Scalar time, String channel, ByteBuffer byteBuffer) { | ||
| if (channel.equals(GokartLcmChannel.STATUS)) { | ||
| GokartStatusEvent gokartStatusEvent = new GokartStatusEvent(byteBuffer); | ||
| gokartRender.gokartStatusListener.getEvent(gokartStatusEvent); | ||
| extrudedFootprintRender.gokartStatusListener.getEvent(gokartStatusEvent); | ||
| } else // | ||
| if (channel.equals(LinmotLcmServer.CHANNEL_GET)) { | ||
| linmotGetEvent = new LinmotGetEvent(byteBuffer); | ||
| gokartRender.linmotGetListener.getEvent(linmotGetEvent); | ||
| } else // | ||
| if (channel.equals(RimoLcmServer.CHANNEL_GET)) { | ||
| RimoGetEvent rimoGetEvent = new RimoGetEvent(byteBuffer); | ||
| gokartRender.rimoGetListener.getEvent(rimoGetEvent); | ||
| tachometerMustangDash.getEvent(rimoGetEvent); | ||
| } else // | ||
| if (channel.equals(RimoLcmServer.CHANNEL_PUT)) { | ||
| RimoPutEvent rimoGetEvent = RimoPutHelper.from(byteBuffer); | ||
| gokartRender.rimoPutListener.putEvent(rimoGetEvent); | ||
| } else // | ||
| if (channel.equals(Vmu931ImuChannel.INSTANCE.channel())) { | ||
| Vmu931ImuFrame vmu931ImuFrame = new Vmu931ImuFrame(byteBuffer); | ||
| accelerationRender.vmu931ImuFrame(vmu931ImuFrame); | ||
| } else // | ||
| if (channel.equals(GokartLcmChannel.MPC_FORCES_CNS)) { | ||
| ControlAndPredictionSteps controlAndPredictionSteps = new ControlAndPredictionStepsMessage(byteBuffer).getPayload(); | ||
| mpcPredictionSequenceRender.getControlAndPredictionSteps(controlAndPredictionSteps); | ||
| mpcPredictionRender.getControlAndPredictionSteps(controlAndPredictionSteps); | ||
| } else // | ||
| if (channel.equals(poseChannel)) { | ||
| GokartPoseEvent gokartPoseEvent = GokartPoseEvent.of(byteBuffer); | ||
| driftLinesRender.getEvent(gokartPoseEvent); | ||
| groundSpeedRender.getEvent(gokartPoseEvent); | ||
| gokartRender.gokartPoseListener.getEvent(gokartPoseEvent); | ||
| extrudedFootprintRender.gokartPoseListener.getEvent(gokartPoseEvent); | ||
| } | ||
| } | ||
|
|
||
| @Override // from RenderInterface | ||
| public void render(GeometricLayer geometricLayer, Graphics2D graphics) { | ||
| mpcPredictionSequenceRender.render(geometricLayer, graphics); | ||
| mpcPredictionRender.render(geometricLayer, graphics); | ||
| driftLinesRender.render(geometricLayer, graphics); | ||
| gokartRender.render(geometricLayer, graphics); | ||
| extrudedFootprintRender.render(geometricLayer, graphics); | ||
| accelerationRender.render(geometricLayer, graphics); | ||
| groundSpeedRender.render(geometricLayer, graphics); | ||
| tachometerMustangDash.render(geometricLayer, graphics); | ||
| // --- | ||
| graphics.setFont(new Font(Font.MONOSPACED, Font.BOLD, 30)); | ||
| graphics.setColor(Color.GRAY); | ||
| if (Objects.nonNull(linmotGetEvent)) | ||
| graphics.drawString(String.format("brake:%12s", linmotGetEvent.getWindingTemperatureMax().map(Round._2)), 0, 25 + 30); | ||
| // --- | ||
| graphics.drawString("vel", 960 - 250 - 140, 25); | ||
| graphics.drawString("acc", 960 + 250 - 140, 25); | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,7 @@ | ||
| // code by jph | ||
| package ch.ethz.idsc.demo.jph.video; | ||
|
|
||
| /* package */ enum StaticHelper { | ||
| ; | ||
| public static final int FRAMERATE = 50; | ||
| } |
Oops, something went wrong.