/// <summary> /// Generates a path around a turn /// </summary> /// <param name="initial"></param> /// <param name="final"></param> /// <param name="turn"></param> /// <param name="vehicleState"></param> /// <param name="vehicleRelative"></param> /// <returns></returns> public static IPath TurnPath(Lane initial, Lane final, Interconnect turn, VehicleState vehicleState, bool vehicleRelative) { // 1. list of absolute positions of lane coordinates List<Coordinates> absoluteCoordinates = new List<Coordinates>(); // make sure not turning onto same lane if(!initial.LaneID.Equals(final.LaneID)) { // 2. loop through initial lane's waypoints absoluteCoordinates.AddRange(GetLaneWaypointCoordinates(initial, true)); // 3. check if the turn has user partitions if (turn.UserPartitions != null) { // 3.1 loop through turn's waypoints if not rndfwaypoints (As will be included by lane coordinate generation foreach (UserPartition partition in turn.UserPartitions) { // add if not an rndf waypoint if (!(partition.InitialWaypoint is RndfWayPoint)) { // add user waypoint's position absoluteCoordinates.Add(partition.InitialWaypoint.Position); } } } // 4. add the next lane's waypoints absoluteCoordinates.AddRange(GetLaneWaypointCoordinates(final, true)); } else { // get lanegth of lane / 4 // HACK double length = RoadToolkit.LanePath(initial, vehicleState, true).Length/4; // 2. loop through initial lane's waypoints absoluteCoordinates.AddRange(GetLaneWaypointCoordinates(initial, true, length, false)); // 3. check if the turn has user partitions if (turn.UserPartitions != null) { // 3.1 loop through turn's waypoints if not rndfwaypoints (As will be included by lane coordinate generation foreach (UserPartition partition in turn.UserPartitions) { // add if not an rndf waypoint if (!(partition.InitialWaypoint is RndfWayPoint)) { // add user waypoint's position absoluteCoordinates.Add(partition.InitialWaypoint.Position); } } } // 4. loop through final lane's waypoints absoluteCoordinates.AddRange(GetLaneWaypointCoordinates(final, true, length, true)); } // 5. Generate path return GeneratePathFromCoordinates(absoluteCoordinates, vehicleState.Position, vehicleState.Heading, vehicleRelative); }
/// <summary> /// Constructor /// </summary> /// <param name="lanePartitionID">Identification information</param> /// <param name="lane">Lane this partition is a part of</param> public LanePartition(LanePartitionID lanePartitionID, Lane lane) { this.lanePartitionID = lanePartitionID; this.lane = lane; }
/// <summary> /// Gets a path for this lane relative to the vehicle (as if the vehicle was travelling to (1,0) /// But only upto a certain distance if the lane goes that far /// </summary> /// <param name="lane"></param> /// <param name="vehicleState"></param> /// <param name="vehicleRelative"></param> /// <returns></returns> public static IPath LanePath(Lane lane, VehicleState vehicleState, double distance, bool forwards, bool vehicleRelative) { // 1. list of absolute positions of lane coordinates List<Coordinates> absoluteCoordinates = new List<Coordinates>(); // 2. get lane coordinates absoluteCoordinates.AddRange(GetLaneWaypointCoordinates(lane, true, distance, forwards)); // 3. Generate path return GeneratePathFromCoordinates(absoluteCoordinates, vehicleState.Position, vehicleState.Heading, vehicleRelative); }
/// <summary> /// Preprocesses the lane paths /// </summary> /// <param name="lane"></param> /// <param name="forwardsPath"></param> /// <param name="backwardsPath"></param> public static void PreprocessLanePaths(Lane lane, out Path forwardsPath, out Path backwardsPath, out List<CubicBezier> forwardSpline) { // 1. list of absolute positions of lane coordinates List<Coordinates> forwardCoordinates = new List<Coordinates>(); // 2. loop through waypoints foreach (RndfWayPoint rwp in lane.Waypoints.Values) { // add position forwardCoordinates.Add(rwp.Position); } // 3. generate spline CubicBezier[] bez = SmoothingSpline.BuildC2Spline(forwardCoordinates.ToArray(), null, null, 0.5); // 4. generate path List<IPathSegment> forwardPathSegments = new List<IPathSegment>(); // spline generation List<CubicBezier> tmpForwardSpline = new List<CubicBezier>(); // 5. loop through individual beziers foreach (CubicBezier cb in bez) { // add to spline tmpForwardSpline.Add(cb); // add to final spline forwardPathSegments.Add(new BezierPathSegment(cb, null, false)); } // set spline forwardSpline = tmpForwardSpline; // 6. Create the forward path forwardsPath = new Path(forwardPathSegments, CoordinateMode.AbsoluteProjected); // 7. list of backwards coordinates List<Coordinates> backwardsCoordinates = forwardCoordinates; backwardsCoordinates.Reverse(); // 8. generate spline bez = SmoothingSpline.BuildC2Spline(backwardsCoordinates.ToArray(), null, null, 0.5); // 9. generate path List<IPathSegment> backwardPathSegments = new List<IPathSegment>(); // 10. loop through individual beziers foreach (CubicBezier cb in bez) { // add to final spline backwardPathSegments.Add(new BezierPathSegment(cb, null, false)); } // 11. generate backwards path backwardsPath = new Path(backwardPathSegments, CoordinateMode.AbsoluteProjected); }
/// <summary> /// Gets up to a certain distance of the lane's coordinates /// </summary> /// <param name="lane"></param> /// <param name="includeUserWaypoint"></param> /// <param name="d"></param> /// <param name="forwards">whether or not to get the front or back of a lane</param> /// <returns></returns> public static List<Coordinates> GetLaneWaypointCoordinates(Lane lane, bool includeUserWaypoints, double d, bool forwards) { // 1. list of absolute positions of lane coordinates List<Coordinates> absoluteCoordinates = new List<Coordinates>(); if (forwards) { RndfWayPoint current = lane.LanePartitions[0].InitialWaypoint; double distance = 0; // loop over waypoints while distance within params while (current != null && distance <= d) { // add rndf waypoint absoluteCoordinates.Add(current.Position); // update a coarse distance if (current.NextLanePartition != null) { // update distance distance += current.NextLanePartition.FinalWaypoint.Position.DistanceTo(current.Position); // check to see if we're including user waypoints if (includeUserWaypoints) { // add the user waypoints foreach (UserPartition userPartition in current.NextLanePartition.UserPartitions) { // check if not an rndfwaypoint if (!(userPartition.InitialWaypoint is RndfWayPoint)) { // add user waypoint's position absoluteCoordinates.Add(userPartition.InitialWaypoint.Position); } } } // iterate current = (current.NextLanePartition.FinalWaypoint); } else { return absoluteCoordinates; } } return absoluteCoordinates; } else { RndfWayPoint current = lane.LanePartitions[lane.LanePartitions.Count-1].FinalWaypoint; double distance = 0; // loop over waypoints while distance within params while (current != null && distance <= d) { // add rndf waypoint absoluteCoordinates.Add(current.Position); // update a coarse distance if (current.PreviousLanePartition != null) { // update distance distance += current.PreviousLanePartition.InitialWaypoint.Position.DistanceTo(current.Position); // check to see if we're including user waypoints if (includeUserWaypoints && current.NextLanePartition != null) { // add the user waypoints foreach (UserPartition userPartition in current.NextLanePartition.UserPartitions) { List<Coordinates> forwardUserCoords = new List<Coordinates>(); // check if not an rndfwaypoint if (!(userPartition.InitialWaypoint is RndfWayPoint)) { // add user waypoint's position forwardUserCoords.Add(userPartition.InitialWaypoint.Position); } forwardUserCoords.Reverse(); absoluteCoordinates.AddRange(forwardUserCoords); } } // iterate current = (current.PreviousLanePartition.InitialWaypoint); } else { absoluteCoordinates.Reverse(); return absoluteCoordinates; } } absoluteCoordinates.Reverse(); return absoluteCoordinates; } }
/// <summary> /// Gets the coordinates of a lane's waypoints /// </summary> /// <param name="includeuserWaypoints"></param> /// <returns></returns> public static List<Coordinates> GetLaneWaypointCoordinates(Lane lane, bool includeuserWaypoints) { // 1. list of absolute positions of lane coordinates List<Coordinates> absoluteCoordinates = new List<Coordinates>(); // 2. loop through initial lane's waypoints foreach (RndfWayPoint waypoint in lane.Waypoints.Values) { // add the waypoint's position absoluteCoordinates.Add(waypoint.Position); // check to see if we're including user waypoints if (includeuserWaypoints && waypoint.NextLanePartition != null) { // add the user waypoints foreach (UserPartition userPartition in waypoint.NextLanePartition.UserPartitions) { // check if not an rndfwaypoint if (!(userPartition.InitialWaypoint is RndfWayPoint)) { // add user waypoint's position absoluteCoordinates.Add(userPartition.InitialWaypoint.Position); } } } } // 3. return coordinates return absoluteCoordinates; }
/// <summary> /// Generates a UTurn Behavior /// </summary> /// <param name="initial"></param> /// <param name="final"></param> /// <returns></returns> public static UTurnBehavior GenerateDefaultUTurnBehavior(Lane initial, Lane final, Interconnect turn, VehicleState vehicleState, bool relative) { // generate polygon /*List<BoundaryLine> boundaryPolygon = GenerateUTurnPolygon(turn.InitialWaypoint, turn.FinalWaypoint); List<Coordinates> boundaryInOrder = new List<Coordinates>(); foreach(BoundaryLine b in boundaryPolygon) boundaryInOrder.Add(b.p1); Polygon polygon = new Polygon(boundaryInOrder, CoordinateMode.AbsoluteProjected); // generate paths IPath initialPath = RoadToolkit.LanePath(initial, vehicleState, relative); IPath finalPath = RoadToolkit.LanePath(final, vehicleState, relative); // generate speeds SpeedCommand initialSpeed = new ScalarSpeedCommand(initial.Way.Segment.SpeedInformation.MaxSpeed); SpeedCommand finalSpeed = new ScalarSpeedCommand(final.Way.Segment.SpeedInformation.MaxSpeed); // generate path behaviors PathFollowingBehavior initialPathBehavior = new PathFollowingBehavior(initialPath, initialSpeed); PathFollowingBehavior finalPathBehavior = new PathFollowingBehavior(finalPath, finalSpeed); // generate U-Turn return new UTurnBehavior(initialPathBehavior, finalPathBehavior, polygon);*/ return null; }
/// <summary> /// Generates a default turn behavior /// </summary> /// <param name="initial"></param> /// <param name="final"></param> /// <param name="turn"></param> /// <param name="vehicleState"></param> /// <param name="relative"></param> /// <returns></returns> public static PathFollowingBehavior GenerateDefaultTurnBehavior(Lane initial, Lane final, Interconnect turn, bool relative) { CubicBezier[] spline; if (turn.UserPartitions.Count == 2) { Coordinates p0 = turn.InitialWaypoint.PreviousLanePartition.InitialWaypoint.Position; Coordinates p1 = turn.InitialWaypoint.Position; Coordinates p2 = turn.UserPartitions[0].FinalWaypoint.Position; Coordinates p3 = turn.FinalWaypoint.Position; Coordinates p4 = turn.FinalWaypoint.NextLanePartition.FinalWaypoint.Position; Coordinates[] pts = { p0, p1, p2, p3, p4 }; spline = SmoothingSpline.BuildC2Spline(pts, null, null, 0.5); } else if (turn.UserPartitions.Count > 2) { Coordinates p0 = turn.InitialWaypoint.PreviousLanePartition.InitialWaypoint.Position; Coordinates p1 = turn.InitialWaypoint.Position; Coordinates p0head = p0 - p1; p0 = p1 + p0head.Normalize(4); List<Coordinates> middleUsers = new List<Coordinates>(); for (int i = 0; i < turn.UserPartitions.Count - 1; i++) { middleUsers.Add(turn.UserPartitions[i].FinalWaypoint.Position); } Coordinates p2 = turn.FinalWaypoint.Position; Coordinates p3 = turn.FinalWaypoint.NextLanePartition.FinalWaypoint.Position; Coordinates p3head = p3 - p2; p3 = p2 + p3head.Normalize(4); List<Coordinates> finalList = new List<Coordinates>(); finalList.Add(p0); finalList.Add(p1); finalList.AddRange(middleUsers); finalList.Add(p2); finalList.Add(p3); spline = SmoothingSpline.BuildC2Spline(finalList.ToArray(), null, null, 0.5); } else { Coordinates p0 = turn.InitialWaypoint.PreviousLanePartition.InitialWaypoint.Position; Coordinates p1 = turn.InitialWaypoint.Position; Coordinates p3 = turn.FinalWaypoint.Position; Coordinates p4 = turn.FinalWaypoint.NextLanePartition.FinalWaypoint.Position; Coordinates[] pts = { p0, p1, p3, p4 }; spline = SmoothingSpline.BuildC2Spline(pts, null, null, 0.5); } // Create the Path Segments List<IPathSegment> bezierPathSegments = new List<IPathSegment>(); foreach (CubicBezier bezier in spline) { bezierPathSegments.Add(new BezierPathSegment(bezier, null, false)); } // get the method from the road toolkit //IPath turnPath = RoadToolkit.TurnPath(initial, final, turn, vehicleState, relative);// CHANGED IPath turnPath = new Path(bezierPathSegments, CoordinateMode.AbsoluteProjected); // make a speed command (set to 2m/s) SpeedCommand speedCommand = new ScalarSpeedCommand(1); // make behavior //return new PathFollowingBehavior(turnPath, speedCommand); return null; }
/// <summary> /// Constructor /// </summary> /// <param name="waypointID">RndfWaypoint Identification Information</param> /// <param name="lane">Lane this wypoint belongs to</param> public RndfWayPoint(RndfWaypointID waypointID, Lane lane) { this.waypointID = waypointID; this.lane = lane; }
/// <summary> /// Constructor /// </summary> /// <param name="waypointID">RndfWaypoint Identification Information</param> /// <param name="lane">Lane this wypoint belongs to</param> public RndfWayPoint(RndfWaypointID waypointID, Lane lane) { this.waypointID = waypointID; this.lane = lane; }
/// <summary> /// Constructor /// </summary> /// <param name="lanePartitionID">Identification information</param> /// <param name="lane">Lane this partition is a part of</param> public LanePartition(LanePartitionID lanePartitionID, Lane lane) { this.lanePartitionID = lanePartitionID; this.lane = lane; }