TestOpenDriveParserNoRTINew.java

  1. package org.opentrafficsim.road.network.factory;

  3. import java.awt.Color;
  4. import java.awt.geom.Rectangle2D;
  5. import java.io.IOException;
  6. import java.net.SocketException;
  7. import java.net.URL;
  8. import java.rmi.RemoteException;
  9. import java.util.ArrayList;
  10. import java.util.Arrays;
  11. import java.util.HashSet;
  12. import java.util.List;
  13. import java.util.Set;

  15. import javax.naming.NamingException;
  16. import javax.swing.JPanel;
  17. import javax.swing.SwingUtilities;
  18. import javax.xml.parsers.ParserConfigurationException;

  20. import nl.javel.gisbeans.io.esri.CoordinateTransform;
  21. import nl.tudelft.simulation.dsol.SimRuntimeException;
  22. import nl.tudelft.simulation.dsol.animation.D2.GisRenderable2D;
  23. import nl.tudelft.simulation.dsol.animation.D2.Renderable2D;
  24. import nl.tudelft.simulation.dsol.simulators.SimulatorInterface;
  25. import nl.tudelft.simulation.jstats.distributions.DistConstant;
  26. import nl.tudelft.simulation.jstats.distributions.DistExponential;
  27. import nl.tudelft.simulation.jstats.distributions.DistTriangular;
  28. import nl.tudelft.simulation.jstats.distributions.DistUniform;
  29. import nl.tudelft.simulation.jstats.streams.MersenneTwister;
  30. import nl.tudelft.simulation.jstats.streams.StreamInterface;
  31. import nl.tudelft.simulation.language.d3.DirectedPoint;
  32. import nl.tudelft.simulation.language.io.URLResource;

  34. import org.djunits.unit.AccelerationUnit;
  35. import org.djunits.unit.LengthUnit;
  36. import org.djunits.unit.SpeedUnit;
  37. import org.djunits.unit.TimeUnit;
  38. import org.djunits.value.vdouble.scalar.Acceleration;
  39. import org.djunits.value.vdouble.scalar.DoubleScalar;
  40. import org.djunits.value.vdouble.scalar.Duration;
  41. import org.djunits.value.vdouble.scalar.Length;
  42. import org.djunits.value.vdouble.scalar.Speed;
  43. import org.djunits.value.vdouble.scalar.Time;
  44. import org.opentrafficsim.core.dsol.OTSDEVSSimulatorInterface;
  45. import org.opentrafficsim.core.dsol.OTSModelInterface;
  46. import org.opentrafficsim.core.dsol.OTSSimTimeDouble;
  47. import org.opentrafficsim.core.geometry.Bezier;
  48. import org.opentrafficsim.core.geometry.OTSGeometryException;
  49. import org.opentrafficsim.core.geometry.OTSLine3D;
  50. import org.opentrafficsim.core.geometry.OTSPoint3D;
  51. import org.opentrafficsim.core.gtu.GTUDirectionality;
  52. import org.opentrafficsim.core.gtu.GTUException;
  53. import org.opentrafficsim.core.gtu.GTUType;
  54. import org.opentrafficsim.core.gtu.animation.AccelerationGTUColorer;
  55. import org.opentrafficsim.core.gtu.animation.GTUColorer;
  56. import org.opentrafficsim.core.gtu.animation.IDGTUColorer;
  57. import org.opentrafficsim.core.gtu.animation.SwitchableGTUColorer;
  58. import org.opentrafficsim.core.gtu.animation.VelocityGTUColorer;
  59. import org.opentrafficsim.core.gtu.behavioralcharacteristics.BehavioralCharacteristics;
  60. import org.opentrafficsim.core.network.Link;
  61. import org.opentrafficsim.core.network.LinkType;
  62. import org.opentrafficsim.core.network.LongitudinalDirectionality;
  63. import org.opentrafficsim.core.network.NetworkException;
  64. import org.opentrafficsim.core.network.Node;
  65. import org.opentrafficsim.core.network.OTSNetwork;
  66. import org.opentrafficsim.core.network.OTSNode;
  67. import org.opentrafficsim.core.network.route.CompleteRoute;
  68. import org.opentrafficsim.core.network.route.Route;
  69. import org.opentrafficsim.core.units.distributions.ContinuousDistDoubleScalar;
  70. import org.opentrafficsim.road.gtu.lane.LaneBasedIndividualGTU;
  71. import org.opentrafficsim.road.gtu.lane.perception.LanePerceptionFull;
  72. import org.opentrafficsim.road.gtu.lane.tactical.LaneBasedGTUFollowingLaneChangeTacticalPlanner;
  73. import org.opentrafficsim.road.gtu.lane.tactical.following.IDMPlusOld;
  74. import org.opentrafficsim.road.gtu.strategical.LaneBasedStrategicalPlanner;
  75. import org.opentrafficsim.road.gtu.strategical.route.LaneBasedStrategicalRoutePlanner;
  76. import org.opentrafficsim.road.network.factory.opendrive.LaneAnimationOD;
  77. import org.opentrafficsim.road.network.factory.opendrive.OpenDriveNetworkLaneParser;
  78. import org.opentrafficsim.road.network.factory.opendrive.communicationRTI.ReceiverThread;
  79. import org.opentrafficsim.road.network.lane.CrossSectionElement;
  80. import org.opentrafficsim.road.network.lane.CrossSectionLink;
  81. import org.opentrafficsim.road.network.lane.DirectedLanePosition;
  82. import org.opentrafficsim.road.network.lane.Lane;
  83. import org.opentrafficsim.road.network.lane.NoTrafficLane;
  84. import org.opentrafficsim.road.network.lane.changing.LaneKeepingPolicy;
  85. import org.opentrafficsim.simulationengine.AbstractWrappableAnimation;
  86. import org.opentrafficsim.simulationengine.OTSSimulationException;
  87. import org.opentrafficsim.simulationengine.properties.AbstractProperty;
  88. import org.opentrafficsim.simulationengine.properties.PropertyException;
  89. import org.xml.sax.SAXException;

  91. /**
  92.  * <p>
  93.  * Copyright (c) 2013-2015 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
  94.  * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
  95.  * <p>
  96.  * $LastChangedDate: 2015-08-05 15:55:21 +0200 (Wed, 05 Aug 2015) $, @version $Revision: 1199 $, by $Author: averbraeck $,
  97.  * initial version Oct 17, 2014 <br>
  98.  * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
  99.  */
  100. public class TestOpenDriveParserNoRTINew extends AbstractWrappableAnimation
  101. {
  102.     /**
  103.      * Main program.
  104.      * @param args String[]; the command line arguments (not used)
  105.      * @throws SimRuntimeException should never happen
  106.      */
  107.     public static void main(final String[] args) throws SimRuntimeException
  108.     {
  109.         SwingUtilities.invokeLater(new Runnable()
  110.         {
  111.             @Override
  112.             public void run()
  113.             {
  114.                 try
  115.                 {
  116.                     TestOpenDriveParserNoRTINew xmlModel = new TestOpenDriveParserNoRTINew();
  117.                     // 1 hour simulation run for testing
  118.                     xmlModel.buildAnimator(new Time(0.0, TimeUnit.SECOND), new Duration(0.0, TimeUnit.SECOND),
  119.                             new Duration(60.0, TimeUnit.MINUTE), new ArrayList<AbstractProperty<?>>(), null, true);
  120.                 }
  121.                 catch (SimRuntimeException | NamingException | OTSSimulationException | PropertyException exception)
  122.                 {
  123.                     exception.printStackTrace();
  124.                 }
  125.             }
  126.         });
  127.     }

  129.     /** {@inheritDoc} */
  130.     @Override
  131.     public final String shortName()
  132.     {
  133.         return "TestOpenDriveModel";
  134.     }

  136.     /** {@inheritDoc} */
  137.     @Override
  138.     public final String description()
  139.     {
  140.         return "TestOpenDriveModel";
  141.     }

  143.     /** {@inheritDoc} */
  144.     @Override
  145.     public final void stopTimersThreads()
  146.     {
  147.         super.stopTimersThreads();
  148.     }

  150.     /** {@inheritDoc} */
  151.     @Override
  152.     protected final JPanel makeCharts()
  153.     {
  154.         return null;
  155.     }

  157.     /** {@inheritDoc} */
  158.     @Override
  159.     protected final OTSModelInterface makeModel(final GTUColorer colorer)
  160.     {
  161.         return new TestOpenDriveModel();
  162.     }

  164.     /** {@inheritDoc} */
  165.     @Override
  166.     protected final Rectangle2D.Double makeAnimationRectangle()
  167.     {
  168.         return new Rectangle2D.Double(-1000, -1000, 2000, 2000);
  169.     }

  171.     /** {@inheritDoc} */
  172.     @Override
  173.     public String toString()
  174.     {
  175.         return "TestOpenDriveParserNoRTINew []";
  176.     }

  178.     /**
  179.      * Model to test the XML parser.
  180.      * <p>
  181.      * Copyright (c) 2013-2015 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. <br>
  182.      * All rights reserved. BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim
  183.      * License</a>.
  184.      * <p>
  185.      * $LastChangedDate: 2015-08-05 15:55:21 +0200 (Wed, 05 Aug 2015) $, @version $Revision: 1199 $, by $Author: averbraeck $,
  186.      * initial version un 27, 2015 <br>
  187.      * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
  188.      * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
  189.      */
  190.     class TestOpenDriveModel implements OTSModelInterface
  191.     {
  192.         /** */
  193.         private static final long serialVersionUID = 20150811L;

  195.         /** The simulator. */
  196.         private OTSDEVSSimulatorInterface simulator;

  198.         private List<LaneBasedIndividualGTU> rtiCars;

  200.         /** */
  201.         private ContinuousDistDoubleScalar.Rel<Speed, SpeedUnit> initialSpeedDist;

  203.         /** */
  204.         private ContinuousDistDoubleScalar.Rel<Duration, TimeUnit> iatDist;

  206.         /** */
  207.         private ContinuousDistDoubleScalar.Rel<Length, LengthUnit> lengthDist;

  209.         /** */
  210.         private ContinuousDistDoubleScalar.Rel<Length, LengthUnit> widthDist;

  212.         /** */
  213.         private ContinuousDistDoubleScalar.Rel<Speed, SpeedUnit> maxSpeedDist;

  215.         /** */
  216.         private ContinuousDistDoubleScalar.Rel<Length, LengthUnit> initialPosDist;

  218.         /** */
  219.         private GTUType carType;

  221.         /** Last id */
  222.         private int lastId = 0;

  224.         /** */
  225.         private StreamInterface stream;

  227.         /**
  228.              * 
  229.              */
  230.         public TestOpenDriveModel()
  231.         {
  232.             super();
  233.             this.stream = new MersenneTwister(1);
  234.             this.initialSpeedDist = new ContinuousDistDoubleScalar.Rel<>(new DistConstant(this.stream, 0.0), SpeedUnit.SI);
  235.             this.iatDist = new ContinuousDistDoubleScalar.Rel<>(new DistExponential(this.stream, 30.0), TimeUnit.SECOND);
  236.             this.lengthDist = new ContinuousDistDoubleScalar.Rel<>(new DistUniform(this.stream, 4.0, 5.0), LengthUnit.METER);
  237.             this.widthDist = new ContinuousDistDoubleScalar.Rel<>(new DistConstant(this.stream, 2.0), LengthUnit.METER);
  238.             this.maxSpeedDist =
  239.                     new ContinuousDistDoubleScalar.Rel<>(new DistTriangular(this.stream, 30.0, 35.0, 40.0),
  240.                             SpeedUnit.MILE_PER_HOUR);
  241.             this.initialPosDist =
  242.                     new ContinuousDistDoubleScalar.Rel<>(new DistUniform(this.stream, 0.0, 1.0), LengthUnit.METER);
  243.             this.carType = new GTUType("Car");
  244.         }

  246.         /** {@inheritDoc} */
  247.         @Override
  248.         public final void constructModel(
  249.                 final SimulatorInterface<DoubleScalar.Abs<TimeUnit>, DoubleScalar.Rel<TimeUnit>, OTSSimTimeDouble> pSimulator)
  250.                 throws SimRuntimeException
  251.         {
  252.             this.simulator = (OTSDEVSSimulatorInterface) pSimulator;

  254.             this.rtiCars = new ArrayList<LaneBasedIndividualGTU>();

  256.             // URL url = URLResource.getResource("/NASAames.xodr");
  257.             URL url = URLResource.getResource("/testod.xodr");
  258.             this.simulator.setPauseOnError(false);
  259.             OpenDriveNetworkLaneParser nlp = new OpenDriveNetworkLaneParser(this.simulator);
  260.             OTSNetwork network = null;
  261.             try
  262.             {
  263.                 network = nlp.build(url);
  264.             }
  265.             catch (NetworkException | ParserConfigurationException | SAXException | IOException | NamingException
  266.                     | GTUException | OTSGeometryException exception)
  267.             {
  268.                 exception.printStackTrace();
  269.             }

  271.             URL gisURL = URLResource.getResource("/gis/map.xml");
  272.             System.err.println("GIS-map file: " + gisURL.toString());

  274.             double latCenter = nlp.getHeaderTag().getOriginLat().si, lonCenter = nlp.getHeaderTag().getOriginLong().si;

  276.             CoordinateTransform latLonToXY = new CoordinateTransformLonLatToXY(lonCenter, latCenter);
  277.             new GisRenderable2D(this.simulator, gisURL, latLonToXY);

  279.             // new ReceiverThread(this.simulator).run();

  281.             /** Repair the network... */
  282.             try
  283.             {
  284.                 destroyLink(nlp, network, "3766109");
  285.                 destroyLink(nlp, network, "3766110");
  286.                 destroyLink(nlp, network, "3766111");

  288.                 Lane lane109 =
  289.                         makeLane(network, "3766068.1", "3766068.0", "3", "3766059.7", "3766059.150", "2", "3766109", "-1",
  290.                                 LinkType.ALL, LaneKeepingPolicy.KEEP_LANE);
  291.                 Renderable2D animation109 = new LaneAnimationOD(lane109, this.simulator, Color.gray);
  292.                 nlp.animationMap.put(lane109, animation109);

  294.                 Lane lane110 =
  295.                         makeLane(network, "3766068.1", "3766068.0", "4", "3766059.7", "3766059.150", "3", "3766110", "-1",
  296.                                 LinkType.ALL, LaneKeepingPolicy.KEEP_LANE);
  297.                 Renderable2D animation110 = new LaneAnimationOD(lane110, this.simulator, Color.gray);
  298.                 nlp.animationMap.put(lane110, animation110);

  300.                 Lane lane111 =
  301.                         makeLane(network, "3766068.1", "3766068.0", "5", "3766059.7", "3766059.150", "4", "3766111", "-1",
  302.                                 LinkType.ALL, LaneKeepingPolicy.KEEP_LANE);
  303.                 Renderable2D animation111 = new LaneAnimationOD(lane111, this.simulator, Color.gray);
  304.                 nlp.animationMap.put(lane111, animation111);

  306.                 destroyLink(nlp, network, "3766175");
  307.                 destroyLink(nlp, network, "3766176");
  308.                 destroyLink(nlp, network, "3766177");

  310.                 Lane lane175 =
  311.                         makeLane(network, "3766059.1", "3766059.0", "3", "3766054.5", "3766054.191", "2", "3766175", "-1",
  312.                                 LinkType.ALL, LaneKeepingPolicy.KEEP_LANE);
  313.                 Renderable2D animation175 = new LaneAnimationOD(lane175, this.simulator, Color.gray);
  314.                 nlp.animationMap.put(lane175, animation175);

  316.                 Lane lane176 =
  317.                         makeLane(network, "3766059.1", "3766059.0", "4", "3766054.5", "3766054.191", "3", "3766176", "-1",
  318.                                 LinkType.ALL, LaneKeepingPolicy.KEEP_LANE);
  319.                 Renderable2D animation176 = new LaneAnimationOD(lane176, this.simulator, Color.gray);
  320.                 nlp.animationMap.put(lane176, animation176);

  322.                 Lane lane177 =
  323.                         makeLane(network, "3766059.1", "3766059.0", "5", "3766054.5", "3766054.191", "4", "3766177", "-1",
  324.                                 LinkType.ALL, LaneKeepingPolicy.KEEP_LANE);
  325.                 Renderable2D animation177 = new LaneAnimationOD(lane177, this.simulator, Color.gray);
  326.                 nlp.animationMap.put(lane177, animation177);

  328.                 Lane lane191x =
  329.                         makeLane(network, "3766054.5", "3766054.191", "-6", "3766059.1", "3766059.0", "-4", "3766191x", "-1",
  330.                                 LinkType.ALL, LaneKeepingPolicy.KEEP_LANE);
  331.                 Renderable2D animation191x = new LaneAnimationOD(lane191x, this.simulator, Color.gray);
  332.                 nlp.animationMap.put(lane191x, animation191x);

  334.             }
  335.             catch (OTSGeometryException | NetworkException | NamingException | RemoteException e)
  336.             {
  337.                 e.printStackTrace();
  338.             }

  340.             /*-
  341.             CompleteRoute cr1 = null, cr2 = null, cr3 = null, cr4 = null, cr5 = null, cr6 = null;

  343.             List<Node> nodesVia1 = new ArrayList<Node>();
  344.             nodesVia1.add(link2.getStartNode());
  345.             nodesVia1.add(link3.getEndNode());
  346.             nodesVia1.add(link4.getStartNode());
  347.             nodesVia1.add(link5.getEndNode());
  348.             nodesVia1.add(link7.getEndNode());
  349.             nodesVia1.add(link8.getStartNode());
  350.             try
  351.             {
  352.                 cr1 =
  353.                     network.getShortestRouteBetween(GTUType.ALL, link1.getStartNode(), link1.getStartNode(), nodesVia1);
  354.                 Collections.reverse(nodesVia1);
  355.                 cr2 =
  356.                     network.getShortestRouteBetween(GTUType.ALL, link1.getStartNode(), link1.getStartNode(), nodesVia1);
  357.             }
  358.             catch (NetworkException exception)
  359.             {
  360.                 exception.printStackTrace();
  361.             }

  363.             List<Node> nodesVia2 = new ArrayList<Node>();
  364.             nodesVia2.add(link3.getEndNode());
  365.             nodesVia2.add(link5.getEndNode());
  366.             try
  367.             {
  368.                 cr3 =
  369.                     network.getShortestRouteBetween(GTUType.ALL, link3.getStartNode(), link3.getStartNode(), nodesVia2);
  370.                 Collections.reverse(nodesVia2);
  371.                 cr4 =
  372.                     network.getShortestRouteBetween(GTUType.ALL, link3.getStartNode(), link3.getStartNode(), nodesVia2);
  373.             }
  374.             catch (NetworkException exception)
  375.             {
  376.                 exception.printStackTrace();
  377.             }

  379.             List<Node> nodesVia3 = new ArrayList<Node>();
  380.             nodesVia3.add(link7.getEndNode());
  381.             nodesVia3.add(link8.getEndNode());
  382.             try
  383.             {
  384.                 cr5 =
  385.                     network.getShortestRouteBetween(GTUType.ALL, link6.getStartNode(), link6.getStartNode(), nodesVia3);
  386.                 Collections.reverse(nodesVia3);
  387.                 cr6 =
  388.                     network.getShortestRouteBetween(GTUType.ALL, link6.getStartNode(), link6.getStartNode(), nodesVia3);
  389.             }
  390.             catch (NetworkException exception)
  391.             {
  392.                 exception.printStackTrace();
  393.             }
  394.              */

  396.             // build the graph
  397.             network.buildGraph(this.carType);

  399.             // generate 2 GTUs on 70.1 (parking lot NRC SV)
  400.             try
  401.             {
  402.                 CrossSectionLink link70_1 = (CrossSectionLink) network.getLink("3766070.1");
  403.                 Lane lane70_1m1 = getLane(link70_1, "-1"); // get the rightmost lane (-1) to drive north
  404.                 Lane lane70_1p1 = getLane(link70_1, "1"); // get the rightmost lane (1) to drive south

  406.                 // create a route around NRC SV
  407.                 Node n70_0 = network.getNode("3766070.0");
  408.                 Node n70_23 = network.getNode("3766070.23");
  409.                 Node n45_0 = network.getNode("3766045.0");
  410.                 Node n45_22 = network.getNode("3766045.22");
  411.                 Node n65_0 = network.getNode("3766065.0");
  412.                 Node n65_8 = network.getNode("3766065.8");
  413.                 Route route7070r =
  414.                         new CompleteRoute("Right around NRC-SV", this.carType, network.getShortestRouteBetween(this.carType,
  415.                                 n70_0, n70_0, Arrays.asList(new Node[] { n45_0, n65_0 })).getNodes());
  416.                 putCar(lane70_1m1, route7070r, network, GTUDirectionality.DIR_PLUS);
  417.                 Route route7070l =
  418.                         new CompleteRoute("Left around NRC-SV", this.carType, network.getShortestRouteBetween(this.carType,
  419.                                 n70_0, n70_0, Arrays.asList(new Node[] { n65_8, n45_22, n70_23 })).getNodes());
  420.                 putCar(lane70_1p1, route7070l, network, GTUDirectionality.DIR_MINUS);
  421.             }
  422.             catch (NetworkException | GTUException | NamingException | OTSGeometryException exception)
  423.             {
  424.                 exception.printStackTrace();
  425.             }

  427.             // generate 10 GTUs on 68 driving the upper circle.
  428.             try
  429.             {
  430.                 List<CrossSectionLink> links68 = new ArrayList<>();
  431.                 links68.add((CrossSectionLink) network.getLink("3766068.1"));
  432.                 links68.add((CrossSectionLink) network.getLink("3766068.2"));
  433.                 links68.add((CrossSectionLink) network.getLink("3766068.3"));
  434.                 links68.add((CrossSectionLink) network.getLink("3766068.4"));
  435.                 links68.add((CrossSectionLink) network.getLink("3766068.5"));

  437.                 // create a right route for top circle
  438.                 Node n68_0 = network.getNode("3766068.0");
  439.                 Node n38_88 = network.getNode("3766038.88");
  440.                 Node n43_0 = network.getNode("3766043.0");
  441.                 Node n45_0 = network.getNode("3766045.0");
  442.                 Node n65_0 = network.getNode("3766065.0");
  443.                 Node n64_17 = network.getNode("3766064.17");
  444.                 Route route6868r =
  445.                         new CompleteRoute("Right top", this.carType, network.getShortestRouteBetween(this.carType, n68_0,
  446.                                 n68_0, Arrays.asList(new Node[] { n38_88, n43_0, n45_0, n65_0, n64_17 })).getNodes());
  447.                 System.out.println(route6868r);
  448.                 for (int i = 0; i < 5; i++)
  449.                 {
  450.                     putCar(randLane(links68, LongitudinalDirectionality.DIR_PLUS), route6868r, network,
  451.                             GTUDirectionality.DIR_PLUS);
  452.                 }

  454.                 // create a left route for top circle
  455.                 Node n38_0 = network.getNode("3766038.0");
  456.                 Node n43_53 = network.getNode("3766043.53");
  457.                 Node n45_22 = network.getNode("3766045.22");
  458.                 Node n65_8 = network.getNode("3766065.8");
  459.                 Node n64_0 = network.getNode("3766064.0");
  460.                 Node n68_158 = network.getNode("3766068.158");
  461.                 Route route6868l =
  462.                         new CompleteRoute("Right top", this.carType, network.getShortestRouteBetween(this.carType, n68_158,
  463.                                 n68_158, Arrays.asList(new Node[] { n64_0, n65_8, n45_22, n43_53, n38_0 })).getNodes());
  464.                 System.out.println(route6868l);
  465.                 for (int i = 0; i < 5; i++)
  466.                 {
  467.                     putCar(randLane(links68, LongitudinalDirectionality.DIR_MINUS), route6868l, network,
  468.                             GTUDirectionality.DIR_MINUS);
  469.                 }

  471.             }
  472.             catch (NetworkException | GTUException | NamingException | OTSGeometryException exception)
  473.             {
  474.                 exception.printStackTrace();
  475.             }

  477.             // XXX dirty hack...
  478.             while (this.rtiCars.size() < 52)
  479.             {
  480.                 this.rtiCars.add(this.rtiCars.get(this.rtiCars.size() - 1));

  482.             }
  483.             /*-
  484.             for (int i = 0; i < 10; i++)
  485.             {
  486.                 Lane lane = null;
  487.                 GTUDirectionality dir = GTUDirectionality.DIR_PLUS;
  488.                 for (CrossSectionElement cse : link1.getCrossSectionElementList())
  489.                 {
  490.                     if (cse instanceof Lane && !(cse instanceof NoTrafficLane))
  491.                     {
  492.                         lane = (Lane) cse;
  493.                         dir =
  494.                             lane.getDirectionality(carType).isForwardOrBoth() ? GTUDirectionality.DIR_PLUS
  495.                                 : GTUDirectionality.DIR_MINUS;
  496.                         break;
  497.                     }
  498.                 }
  499.                 // int i = 1;
  500.                 LaneBasedDrivingCharacteristics drivingCharacteristics =
  501.                     new LaneBasedDrivingCharacteristics(new IDMPlus(), new Altruistic());
  502.                 LaneBasedStrategicalPlanner sPlanner =
  503.                     new LaneBasedStrategicalRoutePlanner(drivingCharacteristics,
  504.                         new LaneBasedGTUFollowingLaneChangeTacticalPlanner(), cr2);

  506.                 System.out.println("Car " + i + " - generated on lane " + lane + " with sn="
  507.                     + lane.getParentLink().getStartNode() + " and en=" + lane.getParentLink().getEndNode()
  508.                     + ", route = " + cr2);

  510.                 LanePerceptionFull perception = new LanePerceptionFull();
  511.                 DirectedLanePosition directedLanePosition =
  512.                     new DirectedLanePosition(lane,
  513.                         initialPosDist.draw().multiplyBy(lane.getCenterLine().getLengthSI()), dir);
  514.                 Set<DirectedLanePosition> lanepositionSet = new HashSet<DirectedLanePosition>();
  515.                 lanepositionSet.add(directedLanePosition);
  516.                 Length carLength = lengthDist.draw();

  518.                 try
  519.                 {
  520.                     LaneBasedIndividualCar car =
  521.                         new LaneBasedIndividualCar(String.valueOf(i), carType, lanepositionSet, new Speed(0.0,
  522.                             SpeedUnit.METER_PER_SECOND), carLength, widthDist.draw(), maxSpeedDist.draw(),
  523.                             this.simulator, sPlanner, perception, network);
  524.                     this.rtiCars.add(car);

  526.                 }
  527.                 catch (NamingException | NetworkException | GTUException | OTSGeometryException exception)
  528.                 {
  529.                     exception.printStackTrace();
  530.                 }
  531.             }
  532.              */

  534.             /*-
  535.             List<CompleteRoute> cRoutes = new ArrayList<>();
  536.             cRoutes.add(cr1);
  537.             cRoutes.add(cr2);
  538.             cRoutes.add(cr3);
  539.             cRoutes.add(cr4);
  540.             cRoutes.add(cr5);
  541.             cRoutes.add(cr6);
  542.             Random routeRandom = new Random();

  544.             List<CrossSectionLink> links = new ArrayList<>();
  545.             links.add(link1);
  546.             links.add(link2);
  547.             links.add(link3);
  548.             links.add(link4);
  549.             links.add(link5);
  550.             links.add(link6);
  551.             links.add(link7);
  552.             links.add(link8);

  554.             for (int i = 0; i < 52; i++)
  555.             {
  556.                 CompleteRoute cr = cRoutes.get(routeRandom.nextInt(6));

  558.                 CrossSectionLink link;
  559.                 while (true)
  560.                 {
  561.                     link = links.get(routeRandom.nextInt(8));
  562.                     if (cr.getNodes().contains(link.getStartNode()))
  563.                         break;
  564.                 }

  566.                 GTUDirectionality dir = GTUDirectionality.DIR_PLUS;
  567.                 Lane lane = null;

  569.                 while (true)
  570.                 {
  571.                     CrossSectionElement cse =
  572.                         link.getCrossSectionElementList().get(
  573.                             routeRandom.nextInt(link.getCrossSectionElementList().size()));
  574.                     if (cse instanceof Lane && !(cse instanceof NoTrafficLane))
  575.                     {
  576.                         lane = (Lane) cse;
  577.                         break;

  579.                     }
  580.                 }

  582.                 if (lane.getDirectionality(carType).equals(LongitudinalDirectionality.DIR_MINUS))
  583.                 {
  584.                     dir = GTUDirectionality.DIR_MINUS;
  585.                 }

  587.                 LaneBasedBehavioralCharacteristics drivingCharacteristics =
  588.                     new LaneBasedBehavioralCharacteristics(new IDMPlusOld(), new Altruistic());
  589.                 LaneBasedStrategicalPlanner sPlanner =
  590.                     new LaneBasedStrategicalRoutePlanner(drivingCharacteristics,
  591.                         new LaneBasedGTUFollowingLaneChangeTacticalPlanner(), cRoutes.get(routeRandom.nextInt(6)));
  592.                 LanePerceptionFull perception = new LanePerceptionFull();

  594.                 DirectedLanePosition directedLanePosition = null;
  595.                 try
  596.                 {
  597.                     directedLanePosition =
  598.                         new DirectedLanePosition(lane, initialPosDist.draw().multiplyBy(
  599.                             lane.getCenterLine().getLengthSI()), dir);
  600.                 }
  601.                 catch (GTUException exception1)
  602.                 {
  603.                     exception1.printStackTrace();
  604.                 }
  605.                 Set<DirectedLanePosition> lanepositionSet = new HashSet<DirectedLanePosition>();
  606.                 lanepositionSet.add(directedLanePosition);

  608.                 Length carLength = lengthDist.draw();
  609.                 double genPosSI = directedLanePosition.getPosition().getSI();
  610.                 double lengthSI = lane.getLength().getSI();
  611.                 double frontNew = (genPosSI + carLength.getSI()) / lengthSI;
  612.                 double rearNew = genPosSI / lengthSI;

  614.                 boolean isEnoughSpace = true;

  616.                 for (LaneBasedGTU gtu : lane.getGtuList())
  617.                 {
  618.                     double frontGTU = 0;
  619.                     try
  620.                     {
  621.                         frontGTU = gtu.fractionalPosition(lane, gtu.getFront());
  622.                     }
  623.                     catch (GTUException exception)
  624.                     {
  625.                         exception.printStackTrace();
  626.                     }
  627.                     double rearGTU = 0;
  628.                     try
  629.                     {
  630.                         rearGTU = gtu.fractionalPosition(lane, gtu.getRear());
  631.                     }
  632.                     catch (GTUException exception)
  633.                     {
  634.                         exception.printStackTrace();
  635.                     }
  636.                     if ((frontNew >= rearGTU && frontNew <= frontGTU) || (rearNew >= rearGTU && rearNew <= frontGTU)
  637.                         || (frontGTU >= rearNew && frontGTU <= frontNew) || (rearGTU >= rearNew && rearGTU <= frontNew))
  638.                         isEnoughSpace = false;
  639.                 }

  641.                 if (isEnoughSpace)
  642.                 {
  643.                     try
  644.                     {
  645.                         LaneBasedIndividualGTU car =
  646.                             new LaneBasedIndividualGTU(String.valueOf(i), carType, lanepositionSet, new Speed(0.0,
  647.                                 SpeedUnit.METER_PER_SECOND), carLength, widthDist.draw(), maxSpeedDist.draw(),
  648.                                 this.simulator, sPlanner, perception, network);
  649.                         this.rtiCars.add(car);

  651.                     }
  652.                     catch (NamingException | NetworkException | GTUException | OTSGeometryException exception)
  653.                     {
  654.                         exception.printStackTrace();
  655.                     }
  656.                 }
  657.                 else
  658.                 {
  659.                     i = i - 1;
  660.                 }

  662.             }
  663.              */

  665.             try
  666.             {
  667.                 new Thread(new ReceiverThread(this.simulator, this.carType, this.rtiCars, network)).start();
  668.             }
  669.             catch (SocketException exception1)
  670.             {
  671.                 exception1.printStackTrace();
  672.             }

  674.         }

  676.         private Lane randLane(final List<CrossSectionLink> links, final LongitudinalDirectionality dir)
  677.         {
  678.             // choose a random link with a random lane on that link in the right direction
  679.             CrossSectionLink link = links.get(this.stream.nextInt(0, links.size() - 1));
  680.             List<Lane> lanes = new ArrayList<>();
  681.             for (CrossSectionElement cse : link.getCrossSectionElementList())
  682.             {
  683.                 if (cse instanceof Lane)
  684.                 {
  685.                     Lane lane = (Lane) cse;
  686.                     if (lane.getDirectionality(this.carType).equals(dir) || lane.getDirectionality(this.carType).isBoth())
  687.                     {
  688.                         lanes.add(lane);
  689.                     }
  690.                 }
  691.             }
  692.             for (int i = 0; i < 10; i++)
  693.             {
  694.                 Lane lane = lanes.get(this.stream.nextInt(0, lanes.size() - 1));
  695.                 if (lane.getGtuList().isEmpty())
  696.                 {
  697.                     return lane;
  698.                 }
  699.             }
  700.             return lanes.get(this.stream.nextInt(0, lanes.size() - 1));
  701.         }

  703.         private void putCar(Lane lane, Route route, OTSNetwork network, GTUDirectionality dir) throws GTUException,
  704.                 NamingException, NetworkException, SimRuntimeException, OTSGeometryException
  705.         {
  706.             BehavioralCharacteristics behavioralCharacteristics = new BehavioralCharacteristics();
  707.             // LaneBasedBehavioralCharacteristics drivingCharacteristics =
  708.             // new LaneBasedBehavioralCharacteristics(new IDMPlusOld(), new Altruistic());
  709.             LaneBasedStrategicalPlanner sPlanner =
  710.                     new LaneBasedStrategicalRoutePlanner(behavioralCharacteristics,
  711.                             new LaneBasedGTUFollowingLaneChangeTacticalPlanner(new IDMPlusOld()), route);
  712.             LanePerceptionFull perception = new LanePerceptionFull();

  714.             DirectedLanePosition directedLanePosition =
  715.                     new DirectedLanePosition(lane, this.initialPosDist.draw().multiplyBy(lane.getCenterLine().getLengthSI()),
  716.                             dir);
  717.             Set<DirectedLanePosition> lanepositionSet = new HashSet<DirectedLanePosition>();
  718.             lanepositionSet.add(directedLanePosition);
  719.             Length carLength = this.lengthDist.draw();
  720.             LaneBasedIndividualGTU car =
  721.                     new LaneBasedIndividualGTU("" + (++this.lastId), this.carType, lanepositionSet, new Speed(0.0,
  722.                             SpeedUnit.METER_PER_SECOND), carLength, this.widthDist.draw(), this.maxSpeedDist.draw(),
  723.                             this.simulator, sPlanner, perception, network);
  724.             this.rtiCars.add(car);
  725.         }

  727.         private Lane getLane(CrossSectionLink link, String id) throws NetworkException
  728.         {
  729.             for (CrossSectionElement cse : link.getCrossSectionElementList())
  730.             {
  731.                 if (cse instanceof Lane && !(cse instanceof NoTrafficLane) && cse.getId().equals(id))
  732.                 {
  733.                     return (Lane) cse;
  734.                 }
  735.             }
  736.             throw new NetworkException("Could not find Lane " + id + " in link " + link);
  737.         }

  739.         /**
  740.          * Destroy the animation of the link and underlying cross section elements
  741.          * @param nlp the parser with the animation map
  742.          * @param network the network in which the link is registered
  743.          * @param linkId the link to destroy
  744.          * @throws NamingException in case destroying fails
  745.          * @throws NetworkException in case link cannot be found in the network
  746.          */
  747.         private void destroyLink(final OpenDriveNetworkLaneParser nlp, final OTSNetwork network, final String linkId)
  748.                 throws NamingException, NetworkException
  749.         {
  750.             Link link = network.getLink(linkId);
  751.             link.getStartNode().removeLink(link);
  752.             link.getEndNode().removeLink(link);
  753.             network.removeLink(link);
  754.             if (link instanceof CrossSectionLink)
  755.             {
  756.                 for (CrossSectionElement cse : ((CrossSectionLink) link).getCrossSectionElementList())
  757.                 {
  758.                     if (nlp.animationMap.containsKey(cse))
  759.                     {
  760.                         nlp.animationMap.get(cse).destroy();
  761.                     }
  762.                 }
  763.             }
  764.             if (nlp.animationMap.containsKey(link))
  765.             {
  766.                 nlp.animationMap.get(link).destroy();
  767.             }
  768.         }

  770.         /**
  771.          * Create an extra link to "repair" the network
  772.          * @param network network
  773.          * @param sLinkStr start link id
  774.          * @param sNodeStr start node id
  775.          * @param sLaneStr start lane id
  776.          * @param eLinkStr end link id
  777.          * @param eNodeStr end node id
  778.          * @param eLaneStr end lane id
  779.          * @param linkId the id of the new link
  780.          * @param laneId the id of the new lane
  781.          * @param linkType the type of the new link
  782.          * @param laneKeepingPolicy the lane keeping policy of the new link
  783.          * @return the created lane
  784.          * @throws OTSGeometryException when points cannot be found or line cannot be constructed
  785.          * @throws NetworkException when lane cannot be constructed
  786.          */
  787.         private Lane makeLane(final OTSNetwork network, final String sLinkStr, final String sNodeStr, final String sLaneStr,
  788.                 final String eLinkStr, final String eNodeStr, final String eLaneStr, final String linkId, final String laneId,
  789.                 final LinkType linkType, final LaneKeepingPolicy laneKeepingPolicy) throws OTSGeometryException,
  790.                 NetworkException
  791.         {
  792.             CrossSectionLink sLink = (CrossSectionLink) network.getLink(sLinkStr);
  793.             OTSNode sNode = (OTSNode) network.getNode(sNodeStr);
  794.             Lane sLane = (Lane) sLink.getCrossSectionElement(sLaneStr);
  795.             CrossSectionLink eLink = (CrossSectionLink) network.getLink(eLinkStr);
  796.             OTSNode eNode = (OTSNode) network.getNode(eNodeStr);
  797.             Lane eLane = (Lane) eLink.getCrossSectionElement(eLaneStr);
  798.             DirectedPoint sp, ep;
  799.             Length beginWidth, endWidth;
  800.             if (sLink.getStartNode().equals(sNode))
  801.             {
  802.                 OTSPoint3D p1 = sLane.getCenterLine().get(1);
  803.                 OTSPoint3D p2 = sLane.getCenterLine().get(0);
  804.                 sp = new DirectedPoint(p2.x, p2.y, p2.z, 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
  805.                 beginWidth = sLane.getBeginWidth();
  806.             }
  807.             else
  808.             {
  809.                 OTSPoint3D p1 = sLane.getCenterLine().get(sLane.getCenterLine().size() - 2);
  810.                 OTSPoint3D p2 = sLane.getCenterLine().get(sLane.getCenterLine().size() - 1);
  811.                 sp = new DirectedPoint(p2.x, p2.y, p2.z, 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
  812.                 beginWidth = sLane.getEndWidth();
  813.             }
  814.             if (eLink.getStartNode().equals(eNode))
  815.             {
  816.                 OTSPoint3D p1 = eLane.getCenterLine().get(1);
  817.                 OTSPoint3D p2 = eLane.getCenterLine().get(0);
  818.                 ep = new DirectedPoint(p2.x, p2.y, p2.z, 0.0, 0.0, Math.atan2(p1.y - p2.y, p1.x - p2.x));
  819.                 endWidth = eLane.getBeginWidth();
  820.             }
  821.             else
  822.             {
  823.                 OTSPoint3D p1 = eLane.getCenterLine().get(eLane.getCenterLine().size() - 2);
  824.                 OTSPoint3D p2 = eLane.getCenterLine().get(eLane.getCenterLine().size() - 1);
  825.                 ep = new DirectedPoint(p2.x, p2.y, p2.z, 0.0, 0.0, Math.atan2(p1.y - p2.y, p1.x - p2.x));
  826.                 endWidth = eLane.getEndWidth();
  827.             }
  828.             OTSLine3D designLine = Bezier.cubic(64, sp, ep);
  829.             CrossSectionLink newLink = new CrossSectionLink(linkId, sNode, eNode, linkType, designLine, laneKeepingPolicy);
  830.             newLink.addDirectionality(GTUType.ALL, LongitudinalDirectionality.DIR_PLUS);
  831.             Lane newLane =
  832.                     new Lane(newLink, laneId, Length.ZERO, Length.ZERO, beginWidth, endWidth, sLane.getLaneType(),
  833.                             LongitudinalDirectionality.DIR_PLUS, sLane.getSpeedLimit(GTUType.ALL),
  834.                             sLane.getOvertakingConditions());
  835.             network.addLink(newLink);
  836.             return newLane;
  837.         }

  839.         /** {@inheritDoc} */
  840.         @Override
  841.         public SimulatorInterface<DoubleScalar.Abs<TimeUnit>, DoubleScalar.Rel<TimeUnit>, OTSSimTimeDouble> getSimulator()

  843.         {
  844.             return this.simulator;
  845.         }

  847.         /**
  848.          * @return a GTUColorer
  849.          */
  850.         private GTUColorer makeSwitchableGTUColorer()
  851.         {
  852.             GTUColorer[] gtuColorers =
  853.                     new GTUColorer[] {
  854.                             new IDGTUColorer(),
  855.                             new VelocityGTUColorer(new Speed(100.0, SpeedUnit.KM_PER_HOUR)),
  856.                             new AccelerationGTUColorer(new Acceleration(1.0, AccelerationUnit.METER_PER_SECOND_2),
  857.                                     new Acceleration(1.0, AccelerationUnit.METER_PER_SECOND_2)) };
  858.             return new SwitchableGTUColorer(0, gtuColorers);
  859.         }

  861.         /** {@inheritDoc} */
  862.         @Override
  863.         public String toString()
  864.         {
  865.             return "TestOpenDriveModel [rtiCar.sizes=" + this.rtiCars.size() + ", carType=" + this.carType + ", lastId="
  866.                     + this.lastId + "]";
  867.         }
  868.     }

  870. }
Created with JaCoCo 0.7.5.201505241946