TestOpenDriveParserSV.java

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

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

  13. import javax.naming.NamingException;
  14. import javax.swing.JPanel;
  15. import javax.swing.SwingUtilities;
  16. import javax.xml.parsers.ParserConfigurationException;

  17. import nl.javel.gisbeans.io.esri.CoordinateTransform;
  18. import nl.tudelft.simulation.dsol.SimRuntimeException;
  19. import nl.tudelft.simulation.dsol.animation.D2.GisRenderable2D;
  20. import nl.tudelft.simulation.dsol.simulators.SimulatorInterface;
  21. import nl.tudelft.simulation.jstats.distributions.DistConstant;
  22. import nl.tudelft.simulation.jstats.distributions.DistExponential;
  23. import nl.tudelft.simulation.jstats.distributions.DistTriangular;
  24. import nl.tudelft.simulation.jstats.distributions.DistUniform;
  25. import nl.tudelft.simulation.jstats.streams.MersenneTwister;
  26. import nl.tudelft.simulation.jstats.streams.StreamInterface;
  27. import nl.tudelft.simulation.language.io.URLResource;

  28. import org.djunits.unit.AccelerationUnit;
  29. import org.djunits.unit.LengthUnit;
  30. import org.djunits.unit.SpeedUnit;
  31. import org.djunits.unit.TimeUnit;
  32. import org.djunits.value.vdouble.scalar.Acceleration;
  33. import org.djunits.value.vdouble.scalar.DoubleScalar;
  34. import org.djunits.value.vdouble.scalar.Duration;
  35. import org.djunits.value.vdouble.scalar.Length;
  36. import org.djunits.value.vdouble.scalar.Speed;
  37. import org.djunits.value.vdouble.scalar.Time;
  38. import org.opentrafficsim.core.dsol.OTSDEVSSimulatorInterface;
  39. import org.opentrafficsim.core.dsol.OTSModelInterface;
  40. import org.opentrafficsim.core.dsol.OTSSimTimeDouble;
  41. import org.opentrafficsim.core.geometry.OTSGeometryException;
  42. import org.opentrafficsim.core.gtu.GTUDirectionality;
  43. import org.opentrafficsim.core.gtu.GTUException;
  44. import org.opentrafficsim.core.gtu.GTUType;
  45. import org.opentrafficsim.core.gtu.animation.AccelerationGTUColorer;
  46. import org.opentrafficsim.core.gtu.animation.GTUColorer;
  47. import org.opentrafficsim.core.gtu.animation.IDGTUColorer;
  48. import org.opentrafficsim.core.gtu.animation.SwitchableGTUColorer;
  49. import org.opentrafficsim.core.gtu.animation.VelocityGTUColorer;
  50. import org.opentrafficsim.core.gtu.behavioralcharacteristics.BehavioralCharacteristics;
  51. import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
  52. import org.opentrafficsim.core.network.Link;
  53. import org.opentrafficsim.core.network.LongitudinalDirectionality;
  54. import org.opentrafficsim.core.network.NetworkException;
  55. import org.opentrafficsim.core.network.Node;
  56. import org.opentrafficsim.core.network.OTSNetwork;
  57. import org.opentrafficsim.core.network.route.CompleteRoute;
  58. import org.opentrafficsim.core.units.distributions.ContinuousDistDoubleScalar;
  59. import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
  60. import org.opentrafficsim.road.gtu.lane.LaneBasedIndividualGTU;
  61. import org.opentrafficsim.road.gtu.lane.perception.LanePerceptionFull;
  62. import org.opentrafficsim.road.gtu.lane.tactical.LaneBasedCFLCTacticalPlanner;
  63. import org.opentrafficsim.road.gtu.lane.tactical.following.IDMPlusOld;
  64. import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.Altruistic;
  65. import org.opentrafficsim.road.gtu.strategical.LaneBasedStrategicalPlanner;
  66. import org.opentrafficsim.road.gtu.strategical.route.LaneBasedStrategicalRoutePlanner;
  67. import org.opentrafficsim.road.network.factory.opendrive.GeneratorAnimation;
  68. import org.opentrafficsim.road.network.factory.opendrive.OpenDriveNetworkLaneParser;
  69. import org.opentrafficsim.road.network.factory.opendrive.communicationRTI.RTICars;
  70. import org.opentrafficsim.road.network.factory.opendrive.communicationRTI.ReceiverThread;
  71. import org.opentrafficsim.road.network.lane.CrossSectionElement;
  72. import org.opentrafficsim.road.network.lane.CrossSectionLink;
  73. import org.opentrafficsim.road.network.lane.DirectedLanePosition;
  74. import org.opentrafficsim.road.network.lane.Lane;
  75. import org.opentrafficsim.road.network.lane.NoTrafficLane;
  76. import org.opentrafficsim.road.network.lane.Sensor;
  77. import org.opentrafficsim.road.network.lane.SinkSensor;
  78. import org.opentrafficsim.simulationengine.AbstractWrappableAnimation;
  79. import org.opentrafficsim.simulationengine.OTSSimulationException;
  80. import org.opentrafficsim.simulationengine.properties.AbstractProperty;
  81. import org.opentrafficsim.simulationengine.properties.PropertyException;
  82. import org.xml.sax.SAXException;

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

  120.     /** {@inheritDoc} */
  121.     @Override
  122.     public final String shortName()
  123.     {
  124.         return "TestOpenDriveModel";
  125.     }

  126.     /** {@inheritDoc} */
  127.     @Override
  128.     public final String description()
  129.     {
  130.         return "TestOpenDriveModel";
  131.     }

  132.     /** {@inheritDoc} */
  133.     @Override
  134.     public final void stopTimersThreads()
  135.     {
  136.         super.stopTimersThreads();
  137.     }

  138.     /** {@inheritDoc} */
  139.     @Override
  140.     protected final JPanel makeCharts()
  141.     {
  142.         return null;
  143.     }

  144.     /** {@inheritDoc} */
  145.     @Override
  146.     protected final OTSModelInterface makeModel(final GTUColorer colorer)
  147.     {
  148.         return new TestOpenDriveModel();
  149.     }

  150.     /** {@inheritDoc} */
  151.     @Override
  152.     protected final Rectangle2D.Double makeAnimationRectangle()
  153.     {
  154.         return new Rectangle2D.Double(-1000, -1000, 2000, 2000);
  155.     }

  156.     /** {@inheritDoc} */
  157.     @Override
  158.     public String toString()
  159.     {
  160.         return "TestOpenDriveParserSV []";
  161.     }

  162.     /**
  163.      * Model to test the XML parser.
  164.      * <p>
  165.      * Copyright (c) 2013-2015 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. <br>
  166.      * All rights reserved. BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim
  167.      * License</a>.
  168.      * <p>
  169.      * $LastChangedDate: 2015-08-05 15:55:21 +0200 (Wed, 05 Aug 2015) $, @version $Revision: 1199 $, by $Author: averbraeck $,
  170.      * initial version un 27, 2015 <br>
  171.      * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
  172.      * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
  173.      */
  174.     class TestOpenDriveModel implements OTSModelInterface
  175.     {
  176.         /** */
  177.         private static final long serialVersionUID = 20150811L;

  178.         /** The simulator. */
  179.         private OTSDEVSSimulatorInterface simulator;

  180.         private List<LaneBasedIndividualGTU> rtiCars;

  181.         /** {@inheritDoc} */
  182.         @Override
  183.         public final
  184.             void
  185.             constructModel(
  186.                 final SimulatorInterface<DoubleScalar.Abs<TimeUnit>, DoubleScalar.Rel<TimeUnit>, OTSSimTimeDouble> pSimulator)
  187.                 throws SimRuntimeException
  188.         {
  189.             this.simulator = (OTSDEVSSimulatorInterface) pSimulator;

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

  191.             // URL url = URLResource.getResource("/NASAames.xodr");
  192.             URL url = URLResource.getResource("/testod.xodr");
  193.             this.simulator.setPauseOnError(false);
  194.             OpenDriveNetworkLaneParser nlp = new OpenDriveNetworkLaneParser(this.simulator);
  195.             OTSNetwork network = null;
  196.             try
  197.             {
  198.                 network = nlp.build(url);
  199.             }
  200.             catch (NetworkException | ParserConfigurationException | SAXException | IOException | NamingException
  201.                 | GTUException | OTSGeometryException exception)
  202.             {
  203.                 exception.printStackTrace();
  204.             }

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

  207.             // TODO parse these from the xodr-file.
  208.             // double latCenter = 37.40897623275873, lonCenter = -122.0246091728831;//sunnyvale
  209.             // double latCenter = 37.419933552777, lonCenter = -122.05752616111000;//nasa
  210.             double latCenter = nlp.getHeaderTag().getOriginLat().si, lonCenter = nlp.getHeaderTag().getOriginLong().si;

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

  213.             // Make a GTU Type
  214.             GTUType carType = new GTUType("Car");

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

  216.             // stream
  217.             StreamInterface stream = new MersenneTwister(1);
  218.             Length M25 = new Length(25.0, LengthUnit.METER);
  219.             Length M0 = new Length(0.0, LengthUnit.METER);

  220.             // distributions
  221.             ContinuousDistDoubleScalar.Rel<Speed, SpeedUnit> initialSpeedDist =
  222.                 new ContinuousDistDoubleScalar.Rel<>(new DistConstant(stream, 0.0), SpeedUnit.SI);
  223.             ContinuousDistDoubleScalar.Rel<Duration, TimeUnit> iatDist =
  224.                 new ContinuousDistDoubleScalar.Rel<>(new DistExponential(stream, 30.0), TimeUnit.SECOND);
  225.             ContinuousDistDoubleScalar.Rel<Length, LengthUnit> lengthDist =
  226.                 new ContinuousDistDoubleScalar.Rel<>(new DistUniform(stream, 4.0, 5.0), LengthUnit.METER);
  227.             ContinuousDistDoubleScalar.Rel<Length, LengthUnit> widthDist =
  228.                 new ContinuousDistDoubleScalar.Rel<>(new DistConstant(stream, 2.0), LengthUnit.METER);
  229.             ContinuousDistDoubleScalar.Rel<Speed, SpeedUnit> maxSpeedDist =
  230.                 new ContinuousDistDoubleScalar.Rel<>(new DistTriangular(stream, 30.0, 35.0, 40.0),
  231.                     SpeedUnit.MILE_PER_HOUR);

  232.             ContinuousDistDoubleScalar.Rel<Length, LengthUnit> initialPosDist =
  233.                 new ContinuousDistDoubleScalar.Rel<>(new DistUniform(stream, 0.0, 1.0), LengthUnit.METER);

  234.             // default colorer

  235.             // put some generators and sinks on the outer edges of the network
  236.             for (Link link : network.getLinkMap().values())
  237.             {
  238.                 CrossSectionLink csLink = (CrossSectionLink) link;
  239.                 // look if start node is isolated
  240.                 if (link.getStartNode().getLinks().size() == 1) // only ourselves...
  241.                 {
  242.                     // put generators and sinks 25 m from the edge of the link
  243.                     for (CrossSectionElement cse : csLink.getCrossSectionElementList())
  244.                     {
  245.                         if (cse instanceof Lane && !(cse instanceof NoTrafficLane))
  246.                         {
  247.                             Lane lane = (Lane) cse;
  248.                             if (Integer.parseInt(lane.getId()) < 0)
  249.                             {
  250.                                 // make a generator
  251.                                 Time startTime = Time.ZERO;
  252.                                 Time endTime = new Time(Double.MAX_VALUE, TimeUnit.SI);
  253.                                 Length position = lane.getLength().lt(M25) ? M0 : M25;
  254.                                 String id = lane.getParentLink().getId() + "." + lane.getId();
  255.                                 BehavioralCharacteristics behavioralCharacteristics = new BehavioralCharacteristics();
  256.                                 //LaneBasedBehavioralCharacteristics drivingCharacteristics =
  257.                                 //    new LaneBasedBehavioralCharacteristics(new IDMPlusOld(), new Altruistic());
  258.                                 try
  259.                                 {
  260.                                     LaneBasedStrategicalPlanner strategicalPlanner =
  261.                                         new LaneBasedStrategicalRoutePlanner(behavioralCharacteristics,
  262.                                             new LaneBasedCFLCTacticalPlanner(new IDMPlusOld(), new Altruistic()));
  263.                                 }
  264.                                 catch (GTUException exception1)
  265.                                 {
  266.                                     throw new SimRuntimeException(exception1);
  267.                                 }
  268.                                 LanePerceptionFull perception = new LanePerceptionFull();
  269.                                 // new GTUGeneratorIndividual(id, this.simulator, carType, LaneBasedIndividualCar.class,
  270.                                 // initialSpeedDist, iatDist, lengthDist, widthDist, maxSpeedDist, Integer.MAX_VALUE,
  271.                                 // startTime, endTime, lane, position, GTUDirectionality.DIR_PLUS,
  272.                                 // makeSwitchableGTUColorer(), strategicalPlanner, perception);
  273.                                 try
  274.                                 {
  275.                                     new GeneratorAnimation(lane, position, this.simulator);
  276.                                 }
  277.                                 catch (RemoteException | NamingException | OTSGeometryException exception)
  278.                                 {
  279.                                     exception.printStackTrace();
  280.                                 }
  281.                             }
  282.                             else
  283.                             {
  284.                                 // make a sink
  285.                                 Length position = lane.getLength().lt(M25) ? M0 : M25;
  286.                                 Sensor sensor = new SinkSensor(lane, position, this.simulator);
  287.                                 try
  288.                                 {
  289.                                     lane.addSensor(sensor, GTUType.ALL);
  290.                                 }
  291.                                 catch (NetworkException exception)
  292.                                 {
  293.                                     exception.printStackTrace();
  294.                                 }
  295.                             }
  296.                         }
  297.                     }
  298.                 }
  299.                 else if (link.getEndNode().getLinks().size() == 1) // only ourselves...
  300.                 {
  301.                     // put generators and sinks 25 m from the edge of the link
  302.                     for (CrossSectionElement cse : csLink.getCrossSectionElementList())
  303.                     {
  304.                         if (cse instanceof Lane && !(cse instanceof NoTrafficLane))
  305.                         {
  306.                             Lane lane = (Lane) cse;
  307.                             if (Integer.parseInt(lane.getId()) > 0)
  308.                             {
  309.                                 // make a generator
  310.                                 Time startTime = Time.ZERO;
  311.                                 Time endTime = new Time(Double.MAX_VALUE, TimeUnit.SI);
  312.                                 Length position =
  313.                                     lane.getLength().lt(M25) ? lane.getLength() : lane.getLength().minus(M25);
  314.                                 String id = lane.getParentLink().getId() + "." + lane.getId();
  315.                                 BehavioralCharacteristics behavioralCharacteristics = new BehavioralCharacteristics();
  316.                                 //LaneBasedBehavioralCharacteristics drivingCharacteristics =
  317.                                 //    new LaneBasedBehavioralCharacteristics(new IDMPlusOld(), new Altruistic());
  318.                                 try
  319.                                 {
  320.                                     LaneBasedStrategicalPlanner strategicalPlanner =
  321.                                         new LaneBasedStrategicalRoutePlanner(behavioralCharacteristics,
  322.                                             new LaneBasedCFLCTacticalPlanner(new IDMPlusOld(), new Altruistic()));
  323.                                 }
  324.                                 catch (GTUException exception1)
  325.                                 {
  326.                                     throw new SimRuntimeException(exception1);
  327.                                 }
  328.                                 LanePerceptionFull perception = new LanePerceptionFull();
  329.                                 // new GTUGeneratorIndividual(id, this.simulator, carType, LaneBasedIndividualCar.class,
  330.                                 // initialSpeedDist, iatDist, lengthDist, widthDist, maxSpeedDist, Integer.MAX_VALUE,
  331.                                 // startTime, endTime, lane, position, GTUDirectionality.DIR_MINUS,
  332.                                 // makeSwitchableGTUColorer(), strategicalPlanner, perception);
  333.                                 try
  334.                                 {
  335.                                     new GeneratorAnimation(lane, position, this.simulator);
  336.                                 }
  337.                                 catch (RemoteException | NamingException | OTSGeometryException exception)
  338.                                 {
  339.                                     exception.printStackTrace();
  340.                                 }
  341.                             }
  342.                             else
  343.                             {
  344.                                 // make a sink
  345.                                 Length position =
  346.                                     lane.getLength().lt(M25) ? lane.getLength() : lane.getLength().minus(M25);
  347.                                 Sensor sensor = new SinkSensor(lane, position, this.simulator);
  348.                                 try
  349.                                 {
  350.                                     lane.addSensor(sensor, GTUType.ALL);
  351.                                 }
  352.                                 catch (NetworkException exception)
  353.                                 {
  354.                                     exception.printStackTrace();
  355.                                 }
  356.                             }
  357.                         }
  358.                     }
  359.                 }
  360.             }

  361.             CrossSectionLink link1 = (CrossSectionLink) network.getLink("3766054.5");
  362.             CrossSectionLink link2 = (CrossSectionLink) network.getLink("3766059.7");
  363.             CrossSectionLink link3 = (CrossSectionLink) network.getLink("3766068.3");
  364.             CrossSectionLink link4 = (CrossSectionLink) network.getLink("3766038.5");
  365.             CrossSectionLink link5 = (CrossSectionLink) network.getLink("3766043.3");
  366.             CrossSectionLink link6 = (CrossSectionLink) network.getLink("3766064.2");
  367.             CrossSectionLink link7 = (CrossSectionLink) network.getLink("3766046.3");
  368.             CrossSectionLink link8 = (CrossSectionLink) network.getLink("3766050.3");

  369.             CompleteRoute cr1 = null, cr2 = null, cr3 = null, cr4 = null, cr5 = null, cr6 = null;

  370.             List<Node> nodesVia1 = new ArrayList<Node>();
  371.             nodesVia1.add(link2.getStartNode());
  372.             nodesVia1.add(link3.getEndNode());
  373.             nodesVia1.add(link4.getStartNode());
  374.             nodesVia1.add(link5.getEndNode());
  375.             nodesVia1.add(link7.getEndNode());
  376.             nodesVia1.add(link8.getStartNode());
  377.             try
  378.             {
  379.                 cr1 =
  380.                     network.getShortestRouteBetween(GTUType.ALL, link1.getStartNode(), link1.getStartNode(), nodesVia1);
  381.                 Collections.reverse(nodesVia1);
  382.                 cr2 =
  383.                     network.getShortestRouteBetween(GTUType.ALL, link1.getStartNode(), link1.getStartNode(), nodesVia1);
  384.             }
  385.             catch (NetworkException exception)
  386.             {
  387.                 exception.printStackTrace();
  388.             }

  389.             List<Node> nodesVia2 = new ArrayList<Node>();
  390.             nodesVia2.add(link3.getEndNode());
  391.             nodesVia2.add(link5.getEndNode());
  392.             try
  393.             {
  394.                 cr3 =
  395.                     network.getShortestRouteBetween(GTUType.ALL, link3.getStartNode(), link3.getStartNode(), nodesVia2);
  396.                 Collections.reverse(nodesVia2);
  397.                 cr4 =
  398.                     network.getShortestRouteBetween(GTUType.ALL, link3.getStartNode(), link3.getStartNode(), nodesVia2);
  399.             }
  400.             catch (NetworkException exception)
  401.             {
  402.                 exception.printStackTrace();
  403.             }

  404.             List<Node> nodesVia3 = new ArrayList<Node>();
  405.             nodesVia3.add(link7.getEndNode());
  406.             nodesVia3.add(link8.getEndNode());
  407.             try
  408.             {
  409.                 cr5 =
  410.                     network.getShortestRouteBetween(GTUType.ALL, link6.getStartNode(), link6.getStartNode(), nodesVia3);
  411.                 Collections.reverse(nodesVia3);
  412.                 cr6 =
  413.                     network.getShortestRouteBetween(GTUType.ALL, link6.getStartNode(), link6.getStartNode(), nodesVia3);
  414.             }
  415.             catch (NetworkException exception)
  416.             {
  417.                 exception.printStackTrace();
  418.             }

  419.             List<CompleteRoute> cRoutes = new ArrayList<>();
  420.             cRoutes.add(cr1);
  421.             cRoutes.add(cr2);
  422.             cRoutes.add(cr3);
  423.             cRoutes.add(cr4);
  424.             cRoutes.add(cr5);
  425.             cRoutes.add(cr6);
  426.             Random routeRandom = new Random();

  427.             List<CrossSectionLink> links = new ArrayList<>();
  428.             links.add(link1);
  429.             links.add(link2);
  430.             links.add(link3);
  431.             links.add(link4);
  432.             links.add(link5);
  433.             links.add(link6);
  434.             links.add(link7);
  435.             links.add(link8);

  436.             for (int i = 0; i < 52; i++)
  437.             {
  438.                 CompleteRoute cr = cRoutes.get(routeRandom.nextInt(6));

  439.                 CrossSectionLink link;
  440.                 while (true)
  441.                 {
  442.                     link = links.get(routeRandom.nextInt(8));
  443.                     if (cr.getNodes().contains(link.getStartNode()))
  444.                         break;
  445.                 }

  446.                 GTUDirectionality dir = GTUDirectionality.DIR_PLUS;
  447.                 Lane lane = null;

  448.                 while (true)
  449.                 {
  450.                     CrossSectionElement cse =
  451.                         link.getCrossSectionElementList().get(
  452.                             routeRandom.nextInt(link.getCrossSectionElementList().size()));
  453.                     if (cse instanceof Lane && !(cse instanceof NoTrafficLane))
  454.                     {
  455.                         lane = (Lane) cse;
  456.                         break;

  457.                     }
  458.                 }

  459.                 if (lane.getDirectionality(carType).equals(LongitudinalDirectionality.DIR_MINUS))
  460.                 {
  461.                     dir = GTUDirectionality.DIR_MINUS;
  462.                 }

  463.                 BehavioralCharacteristics behavioralCharacteristics = new BehavioralCharacteristics();
  464.                 //LaneBasedBehavioralCharacteristics drivingCharacteristics =
  465.                 //    new LaneBasedBehavioralCharacteristics(new IDMPlusOld(), new Altruistic());
  466.                 LaneBasedStrategicalPlanner sPlanner;
  467.                 try
  468.                 {
  469.                     sPlanner = new LaneBasedStrategicalRoutePlanner(behavioralCharacteristics,
  470.                         new LaneBasedCFLCTacticalPlanner(new IDMPlusOld(), new Altruistic()));
  471.                 }
  472.                 catch (GTUException exception2)
  473.                 {
  474.                     throw new SimRuntimeException(exception2);
  475.                 }
  476.                 LanePerceptionFull perception = new LanePerceptionFull();

  477.                 DirectedLanePosition directedLanePosition = null;
  478.                 try
  479.                 {
  480.                     directedLanePosition =
  481.                         new DirectedLanePosition(lane, initialPosDist.draw().multiplyBy(
  482.                             lane.getCenterLine().getLengthSI()), dir);
  483.                 }
  484.                 catch (GTUException exception1)
  485.                 {
  486.                     exception1.printStackTrace();
  487.                 }
  488.                 Set<DirectedLanePosition> lanepositionSet = new HashSet<DirectedLanePosition>();
  489.                 lanepositionSet.add(directedLanePosition);

  490.                 Length carLength = lengthDist.draw();
  491.                 double genPosSI = directedLanePosition.getPosition().getSI();
  492.                 double lengthSI = lane.getLength().getSI();
  493.                 double frontNew = (genPosSI + carLength.getSI()) / lengthSI;
  494.                 double rearNew = genPosSI / lengthSI;

  495.                 boolean isEnoughSpace = true;

  496.                 for (LaneBasedGTU gtu : lane.getGtuList())
  497.                 {
  498.                     double frontGTU = 0;
  499.                     try
  500.                     {
  501.                         frontGTU = gtu.fractionalPosition(lane, gtu.getFront());
  502.                     }
  503.                     catch (GTUException exception)
  504.                     {
  505.                         exception.printStackTrace();
  506.                     }
  507.                     double rearGTU = 0;
  508.                     try
  509.                     {
  510.                         rearGTU = gtu.fractionalPosition(lane, gtu.getRear());
  511.                     }
  512.                     catch (GTUException exception)
  513.                     {
  514.                         exception.printStackTrace();
  515.                     }
  516.                     if ((frontNew >= rearGTU && frontNew <= frontGTU) || (rearNew >= rearGTU && rearNew <= frontGTU)
  517.                         || (frontGTU >= rearNew && frontGTU <= frontNew) || (rearGTU >= rearNew && rearGTU <= frontNew))
  518.                         isEnoughSpace = false;
  519.                 }

  520.                 if (isEnoughSpace)
  521.                 {
  522.                     try
  523.                     {
  524.                         RTICars car =
  525.                             new RTICars(String.valueOf(i), carType, lanepositionSet, new Speed(0.0,
  526.                                 SpeedUnit.METER_PER_SECOND), carLength, widthDist.draw(), maxSpeedDist.draw(),
  527.                                 this.simulator, sPlanner, perception, network);
  528.                         this.rtiCars.add(car);

  529.                     }
  530.                     catch (NamingException | NetworkException | GTUException | OperationalPlanException
  531.                         | OTSGeometryException exception)
  532.                     {
  533.                         exception.printStackTrace();
  534.                     }
  535.                 }
  536.                 else
  537.                 {
  538.                     i = i - 1;
  539.                 }

  540.             }

  541.             try
  542.             {
  543.                 new Thread(new ReceiverThread(this.simulator, carType, this.rtiCars, network)).start();
  544.             }
  545.             catch (SocketException exception1)
  546.             {
  547.                 exception1.printStackTrace();
  548.             }

  549.             /*
  550.              * CrossSectionLink link = (CrossSectionLink) network.getLink("3766053"); for (CrossSectionElement cse :
  551.              * link.getCrossSectionElementList()) { if (cse instanceof Lane) { Lane lane = (Lane) cse;
  552.              * System.out.println("Lane " + lane + " - offset=" + lane.getDesignLineOffsetAtEnd() + " - nextlanes(ALL) = " +
  553.              * lane.nextLanes(GTUType.ALL) + " - nextlanes(CarType) = " + lane.nextLanes(carType)); } }
  554.              */
  555.             // test the shortest path method
  556.             /*
  557.              * Node nodeFrom = network.getLink("3766052").getEndNode(); Node nodeTo =
  558.              * network.getLink("3766035.1").getStartNode(); CompleteRoute cr; try { cr =
  559.              * network.getShortestRouteBetween(GTUType.ALL, nodeFrom, nodeTo);
  560.              * System.out.println(cr.toString().replaceAll("to OTSNode", "\ntoOTSNode").replaceAll(", OTSNode", ", \nOTSNode"));
  561.              * } catch (NetworkException exception) { exception.printStackTrace(); }
  562.              */
  563.         }

  564.         /** {@inheritDoc} */
  565.         @Override
  566.         public SimulatorInterface<DoubleScalar.Abs<TimeUnit>, DoubleScalar.Rel<TimeUnit>, OTSSimTimeDouble>
  567.             getSimulator()

  568.         {
  569.             return this.simulator;
  570.         }

  571.         /**
  572.          * @return a GTUColorer
  573.          */
  574.         private GTUColorer makeSwitchableGTUColorer()
  575.         {
  576.             GTUColorer[] gtuColorers =
  577.                 new GTUColorer[]{
  578.                     new IDGTUColorer(),
  579.                     new VelocityGTUColorer(new Speed(100.0, SpeedUnit.KM_PER_HOUR)),
  580.                     new AccelerationGTUColorer(new Acceleration(1.0, AccelerationUnit.METER_PER_SECOND_2),
  581.                         new Acceleration(1.0, AccelerationUnit.METER_PER_SECOND_2))};
  582.             return new SwitchableGTUColorer(0, gtuColorers);
  583.         }

  584.         /** {@inheritDoc} */
  585.         @Override
  586.         public final String toString()
  587.         {
  588.             return "TestOpenDriveModel [simulator=" + this.simulator + ", rtiCars.size=" + this.rtiCars.size() + "]";
  589.         }
  590.     }

  591. }