private bool CheckGeneralShape(LinePath rndfPath, CarTimestamp rndfPathTimestamp, LocalLaneModel centerLaneModel) { // bad newz bears if (centerLaneModel.LanePath == null || centerLaneModel.LanePath.Count < 2) { return(false); } // project the lane model's path into the rndf path's timestamp RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp); LinePath laneModelPath = centerLaneModel.LanePath.Transform(relTransform); // get the zero point of the lane model path LinePath.PointOnPath laneZeroPoint = laneModelPath.ZeroPoint; // get the first waypoint on the RNDF path LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint; Coordinates firstWaypoint = rndfPath[rndfZeroPoint.Index + 1]; // get the projection of the first waypoint onto the lane path LinePath.PointOnPath laneFirstWaypoint = laneModelPath.GetClosestPoint(firstWaypoint); // get the offset vector Coordinates offsetVector = laneFirstWaypoint.Location - firstWaypoint; // start iterating through the waypoints forward and project them onto the rndf path for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++) { // get the waypoint Coordinates waypoint = rndfPath[i]; // adjust by the first waypoint offset waypoint += offsetVector; // project onto the lane model LinePath.PointOnPath laneWaypoint = laneModelPath.GetClosestPoint(waypoint); // check the distance from the zero point on the lane model if (laneModelPath.DistanceBetween(laneZeroPoint, laneWaypoint) > road_model_max_dist) { break; } // check the devation from the rndf double deviation = waypoint.DistanceTo(laneWaypoint.Location); // if the deviation is over some threshold, then we reject the model if (deviation > road_deviation_reject_threshold) { return(false); } } // we got this far, so this stuff is OK return(true); }
public static void CalculateMaxPathSpeed(LinePath path, LinePath.PointOnPath startPoint, LinePath.PointOnPath endPoint, ref double maxSpeed) { // get the angles List<Pair<int, double>> angles = path.GetIntersectionAngles(startPoint.Index, endPoint.Index); foreach (Pair<int, double> angleValue in angles) { // calculate the desired speed to take the point double dist = path.DistanceBetween(startPoint, path.GetPointOnPath(angleValue.Left)); // calculate the desired speed at the intersection double desiredSpeed = 1.5/(angleValue.Right/(Math.PI/2)); // limit the desired speed to 2.5 desiredSpeed = Math.Max(2.5, desiredSpeed); // calculate the speed we would be allowed to go right now double desiredMaxSpeed = Math.Sqrt(desiredSpeed*desiredSpeed + 2*target_decel*dist); // check if the desired max speed is lower if (desiredMaxSpeed < maxSpeed) { maxSpeed = desiredMaxSpeed; } } }
public static void CalculateMaxPathSpeed(LinePath path, LinePath.PointOnPath startPoint, LinePath.PointOnPath endPoint, ref double maxSpeed) { // get the angles List <Pair <int, double> > angles = path.GetIntersectionAngles(startPoint.Index, endPoint.Index); foreach (Pair <int, double> angleValue in angles) { // calculate the desired speed to take the point double dist = path.DistanceBetween(startPoint, path.GetPointOnPath(angleValue.Left)); // calculate the desired speed at the intersection double desiredSpeed = 1.5 / (angleValue.Right / (Math.PI / 2)); // limit the desired speed to 2.5 desiredSpeed = Math.Max(2.5, desiredSpeed); // calculate the speed we would be allowed to go right now double desiredMaxSpeed = Math.Sqrt(desiredSpeed * desiredSpeed + 2 * target_decel * dist); // check if the desired max speed is lower if (desiredMaxSpeed < maxSpeed) { maxSpeed = desiredMaxSpeed; } } }
private ILaneModel GetLaneModel(LocalLaneModel laneModel, LinePath rndfPath, double rndfPathWidth, CarTimestamp rndfPathTimestamp) { // check the lane model probability if (laneModel.Probability < lane_probability_reject_threshold) { // we're rejecting this, just return a path lane model return(new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth)); } // project the lane model's path into the rndf path's timestamp RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp); LinePath laneModelPath = laneModel.LanePath.Transform(relTransform); // iterate through the waypoints in the RNDF path and project onto the lane model // the first one that is over the threshold, we consider the waypoint before as a potential ending point LinePath.PointOnPath laneModelDeviationEndPoint = new LinePath.PointOnPath(); // flag indicating if any of the waypoint tests failed because of the devation was too high bool anyDeviationTooHigh = false; // number of waypoints accepted int numWaypointsAccepted = 0; // get the vehicle's position on the rndf path LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint; // get the vehicle's position on the lane model LinePath.PointOnPath laneModelZeroPoint = laneModelPath.ZeroPoint; // get the last point we want to consider on the lane model LinePath.PointOnPath laneModelFarthestPoint = laneModelPath.AdvancePoint(laneModelZeroPoint, lane_model_max_dist); // start walking forward through the waypoints on the rndf path // this loop will implicitly exit when we're past the end of the lane model as the waypoints // will stop being close to the lane model (GetClosestPoint returns the end point if we're past the // end of the path) for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++) { // get the waypoint Coordinates rndfWaypoint = rndfPath[i]; // get the closest point on the lane model LinePath.PointOnPath laneModelClosestPoint = laneModelPath.GetClosestPoint(rndfWaypoint); // compute the distance between the two double deviation = rndfWaypoint.DistanceTo(laneModelClosestPoint.Location); // if this is above the deviation threshold, leave the loop if (deviation > lane_deviation_reject_threshold || laneModelClosestPoint > laneModelFarthestPoint) { // if we're at the end of the lane model path, we don't want to consider this a rejection if (laneModelClosestPoint < laneModelFarthestPoint) { // mark that at least on deviation was too high anyDeviationTooHigh = true; } break; } // increment the number of waypoint accepted numWaypointsAccepted++; // update the end point of where we're valid as the local road model was OK up to this point laneModelDeviationEndPoint = laneModelClosestPoint; } // go through and figure out how far out the variance is within tolerance LinePath.PointOnPath laneModelVarianceEndPoint = new LinePath.PointOnPath(); // walk forward from this point until the end of the lane mode path for (int i = laneModelZeroPoint.Index + 1; i < laneModelPath.Count; i++) { // check if we're within the variance toleration if (laneModel.LaneYVariance[i] <= y_var_reject_threshold) { // we are, update the point on path laneModelVarianceEndPoint = laneModelPath.GetPointOnPath(i); } else { // we are out of tolerance, break out of the loop break; } } // now figure out everything out // determine waypoint rejection status WaypointRejectionResult waypointRejectionResult; if (laneModelDeviationEndPoint.Valid) { // if the point is valid, that we had at least one waypoint that was ok // check if any waypoints were rejected if (anyDeviationTooHigh) { // some waypoint was ok, so we know that at least one waypoint was accepted waypointRejectionResult = WaypointRejectionResult.SomeWaypointsAccepted; } else { // no waypoint triggered a rejection, but at least one was good waypointRejectionResult = WaypointRejectionResult.AllWaypointsAccepted; } } else { // the point is not valid, so we either had no waypoints or we had all rejections if (anyDeviationTooHigh) { // the first waypoint was rejected, so all are rejected waypointRejectionResult = WaypointRejectionResult.AllWaypointsRejected; } else { // the first waypoint (if any) was past the end of the lane model waypointRejectionResult = WaypointRejectionResult.NoWaypoints; } } // criteria for determining if this path is valid: // - if some or all waypoints were accepted, than this is probably a good path // - if some of the waypoints were accepted, we go no farther than the last waypoint that was accepted // - if there were no waypoints, this is a potentially dangerous situation since we can't reject // or confirm the local road model. for now, we'll assume that it is correct but this may need to change // if we start handling intersections in this framework // - if all waypoints were rejected, than we don't use the local road model // - go no farther than the laneModelVarianceEndPoint, which is the last point where the y-variance of // the lane model was in tolerance // now build out the lane model ILaneModel finalLaneModel; // check if we rejected all waypoints or no lane model points satisified the variance threshold if (waypointRejectionResult == WaypointRejectionResult.AllWaypointsRejected || !laneModelVarianceEndPoint.Valid) { // want to just use the path lane model finalLaneModel = new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth); } else { // we'll use the lane model // need to build up the center line as well as left and right bounds // to build up the center line, use the lane model as far as we feel comfortable (limited by either variance // or by rejections) and then use the rndf lane after that. LinePath centerLine = new LinePath(); // figure out the max distance // if there were no waypoints, set the laneModelDeviationEndPoint to the end of the lane model if (waypointRejectionResult == WaypointRejectionResult.NoWaypoints) { laneModelDeviationEndPoint = laneModelFarthestPoint; } // figure out the closer of the end points LinePath.PointOnPath laneModelEndPoint = (laneModelDeviationEndPoint < laneModelVarianceEndPoint) ? laneModelDeviationEndPoint : laneModelVarianceEndPoint; bool endAtWaypoint = laneModelEndPoint == laneModelDeviationEndPoint; // add the lane model to the center line centerLine.AddRange(laneModelPath.GetSubpathEnumerator(laneModelZeroPoint, laneModelEndPoint)); // create a list to hold the width expansion values List <double> widthValue = new List <double>(); // make the width expansion values the width of the path plus the 1-sigma values for (int i = laneModelZeroPoint.Index; i < laneModelZeroPoint.Index + centerLine.Count; i++) { widthValue.Add(laneModel.Width / 2.0 + Math.Sqrt(laneModel.LaneYVariance[i])); } // now figure out how to add the rndf path // get the projection of the lane model end point on the rndf path LinePath.PointOnPath rndfPathStartPoint = rndfPath.GetClosestPoint(laneModelEndPoint.Location); // if the closest point is past the end of rndf path, then we don't want to tack anything on if (rndfPathStartPoint != rndfPath.EndPoint) { // get the last segment of the new center line Coordinates centerLineEndSegmentVec = centerLine.EndSegment.UnitVector; // get the last point of the new center line Coordinates laneModelEndLoc = laneModelEndPoint.Location; // now figure out the distance to the next waypoint LinePath.PointOnPath rndfNextPoint = new LinePath.PointOnPath(); // figure out if we're ending at a waypoint or not if (endAtWaypoint) { rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index + 1); // if the distance from the start point to the next point is less than rndf_dist_min, then // use the waypont after double dist = rndfPath.DistanceBetween(rndfPathStartPoint, rndfNextPoint); if (dist < rndf_dist_min) { if (rndfPathStartPoint.Index < rndfPath.Count - 2) { rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index + 2); } else if (rndfPath.DistanceBetween(rndfPathStartPoint, rndfPath.EndPoint) < rndf_dist_min) { rndfNextPoint = LinePath.PointOnPath.Invalid; } else { rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, rndf_dist_min * 2); } } } else { // track the last angle we had double lastAngle = double.NaN; // walk down the rndf path until we find a valid point for (double dist = rndf_dist_min; dist <= rndf_dist_max; dist += rndf_dist_step) { // advance from the start point by dist double distTemp = dist; rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, ref distTemp); // if the distTemp is > 0, then we're past the end of the path if (distTemp > 0) { // if we're immediately past the end, we don't want to tack anything on if (dist == rndf_dist_min) { rndfNextPoint = LinePath.PointOnPath.Invalid; } break; } // check the angle made by the last segment of center line and the segment // formed between the end point of the center line and this new point double angle = Math.Acos(centerLineEndSegmentVec.Dot((rndfNextPoint.Location - laneModelEndLoc).Normalize())); // check if the angle satisfies the threshold or we're increasing the angle if (Math.Abs(angle) < rndf_angle_threshold || (!double.IsNaN(lastAngle) && angle > lastAngle)) { // break out of the loop, we're done searching break; } lastAngle = angle; } } // tack on the rndf starting at next point going to the end if (rndfNextPoint.Valid) { LinePath subPath = rndfPath.SubPath(rndfNextPoint, rndfPath.EndPoint); centerLine.AddRange(subPath); // insert the lane model end point into the sub path subPath.Insert(0, laneModelEndLoc); // get the angles List <Pair <int, double> > angles = subPath.GetIntersectionAngles(0, subPath.Count - 1); // add the width of the path inflated by the angles for (int i = 0; i < angles.Count; i++) { // calculate the width expansion factor // 90 deg, 3x width // 45 deg, 1.5x width // 0 deg, 1x width double widthFactor = Math.Pow(angles[i].Right / (Math.PI / 2.0), 2) * 2 + 1; // add the width value widthValue.Add(widthFactor * laneModel.Width / 2); } // add the final width widthValue.Add(laneModel.Width / 2); } // set the rndf path start point to be the point we used rndfPathStartPoint = rndfNextPoint; } // for now, calculate the left and right bounds the same way we do for the path lane model // TODO: figure out if we want to do this more intelligently using knowledge of the lane model uncertainty LinePath leftBound = centerLine.ShiftLateral(widthValue.ToArray()); // get the next shifts for (int i = 0; i < widthValue.Count; i++) { widthValue[i] = -widthValue[i]; } LinePath rightBound = centerLine.ShiftLateral(widthValue.ToArray()); // build the final lane model finalLaneModel = new CombinedLaneModel(centerLine, leftBound, rightBound, laneModel.Width, rndfPathTimestamp); } SendLaneModelToUI(finalLaneModel, rndfPathTimestamp); // output the fit result return(finalLaneModel); }
private LaneModelTestResult TestLane(LinePath rndfPath, CarTimestamp rndfPathTimestamp, LocalLaneModel laneModel) { // construct the result object to hold stuff LaneModelTestResult result = new LaneModelTestResult(); // project the lane model's path into the rndf path's timestamp RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp); LinePath laneModelPath = laneModel.LanePath.Transform(relTransform); // get the zero point of the lane model path LinePath.PointOnPath laneZeroPoint = laneModelPath.ZeroPoint; // get the first waypoint on the RNDF path LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint; // get the heading of the rndf path at its zero point and the heading of the lane model at // the rndf's zero point LinePath.PointOnPath laneStartPoint = laneModelPath.GetClosestPoint(rndfZeroPoint.Location); Coordinates laneModelHeading = laneModelPath.GetSegment(laneStartPoint.Index).UnitVector; Coordinates rndfHeading = rndfPath.GetSegment(rndfZeroPoint.Index).UnitVector; double angle = Math.Acos(laneModelHeading.Dot(rndfHeading)); // check if the angle is within limits for comparing offset if (angle < 30 * Math.PI / 180.0) { // get the deviation between lane zero point and rndf zero point result.rndf_deviation = rndfZeroPoint.Location.DistanceTo(laneZeroPoint.Location); } // now start check for how many waypoints are accepted for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++) { // check the distance along the rndf path double rndfDistAlong = rndfPath.DistanceBetween(rndfZeroPoint, rndfPath.GetPointOnPath(i)); // break out if we're too far along the rndf if (rndfDistAlong > 50) { break; } // get the waypoint Coordinates waypoint = rndfPath[i]; // project on to lane path LinePath.PointOnPath laneWaypoint = laneModelPath.GetClosestPoint(waypoint); // check if we're too far along the lane path double distAlong = laneModelPath.DistanceBetween(laneZeroPoint, laneWaypoint); if (distAlong > lane_model_max_dist || distAlong < 0) { break; } // check if the deviation double dist = waypoint.DistanceTo(laneWaypoint.Location); // increment appropriate counts if (dist < lane_deviation_reject_threshold) { result.forward_match_count++; } else { result.forward_rejection_count++; } } // return the result return(result); }
private LaneModelTestResult TestLane(LinePath rndfPath, CarTimestamp rndfPathTimestamp, LocalLaneModel laneModel) { // construct the result object to hold stuff LaneModelTestResult result = new LaneModelTestResult(); // project the lane model's path into the rndf path's timestamp RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp); LinePath laneModelPath = laneModel.LanePath.Transform(relTransform); // get the zero point of the lane model path LinePath.PointOnPath laneZeroPoint = laneModelPath.ZeroPoint; // get the first waypoint on the RNDF path LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint; // get the heading of the rndf path at its zero point and the heading of the lane model at // the rndf's zero point LinePath.PointOnPath laneStartPoint = laneModelPath.GetClosestPoint(rndfZeroPoint.Location); Coordinates laneModelHeading = laneModelPath.GetSegment(laneStartPoint.Index).UnitVector; Coordinates rndfHeading = rndfPath.GetSegment(rndfZeroPoint.Index).UnitVector; double angle = Math.Acos(laneModelHeading.Dot(rndfHeading)); // check if the angle is within limits for comparing offset if (angle < 30*Math.PI/180.0) { // get the deviation between lane zero point and rndf zero point result.rndf_deviation = rndfZeroPoint.Location.DistanceTo(laneZeroPoint.Location); } // now start check for how many waypoints are accepted for (int i = rndfZeroPoint.Index + 1; i < rndfPath.Count; i++) { // check the distance along the rndf path double rndfDistAlong = rndfPath.DistanceBetween(rndfZeroPoint, rndfPath.GetPointOnPath(i)); // break out if we're too far along the rndf if (rndfDistAlong > 50) { break; } // get the waypoint Coordinates waypoint = rndfPath[i]; // project on to lane path LinePath.PointOnPath laneWaypoint = laneModelPath.GetClosestPoint(waypoint); // check if we're too far along the lane path double distAlong = laneModelPath.DistanceBetween(laneZeroPoint, laneWaypoint); if (distAlong > lane_model_max_dist || distAlong < 0) { break; } // check if the deviation double dist = waypoint.DistanceTo(laneWaypoint.Location); // increment appropriate counts if (dist < lane_deviation_reject_threshold) { result.forward_match_count++; } else { result.forward_rejection_count++; } } // return the result return result; }
private ILaneModel GetLaneModel(LocalLaneModel laneModel, LinePath rndfPath, double rndfPathWidth, CarTimestamp rndfPathTimestamp) { // check the lane model probability if (laneModel.Probability < lane_probability_reject_threshold) { // we're rejecting this, just return a path lane model return new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth); } // project the lane model's path into the rndf path's timestamp RelativeTransform relTransform = Services.RelativePose.GetTransform(localRoadModel.Timestamp, rndfPathTimestamp); LinePath laneModelPath = laneModel.LanePath.Transform(relTransform); // iterate through the waypoints in the RNDF path and project onto the lane model // the first one that is over the threshold, we consider the waypoint before as a potential ending point LinePath.PointOnPath laneModelDeviationEndPoint = new LinePath.PointOnPath(); // flag indicating if any of the waypoint tests failed because of the devation was too high bool anyDeviationTooHigh = false; // number of waypoints accepted int numWaypointsAccepted = 0; // get the vehicle's position on the rndf path LinePath.PointOnPath rndfZeroPoint = rndfPath.ZeroPoint; // get the vehicle's position on the lane model LinePath.PointOnPath laneModelZeroPoint = laneModelPath.ZeroPoint; // get the last point we want to consider on the lane model LinePath.PointOnPath laneModelFarthestPoint = laneModelPath.AdvancePoint(laneModelZeroPoint, lane_model_max_dist); // start walking forward through the waypoints on the rndf path // this loop will implicitly exit when we're past the end of the lane model as the waypoints // will stop being close to the lane model (GetClosestPoint returns the end point if we're past the // end of the path) for (int i = rndfZeroPoint.Index+1; i < rndfPath.Count; i++) { // get the waypoint Coordinates rndfWaypoint = rndfPath[i]; // get the closest point on the lane model LinePath.PointOnPath laneModelClosestPoint = laneModelPath.GetClosestPoint(rndfWaypoint); // compute the distance between the two double deviation = rndfWaypoint.DistanceTo(laneModelClosestPoint.Location); // if this is above the deviation threshold, leave the loop if (deviation > lane_deviation_reject_threshold || laneModelClosestPoint > laneModelFarthestPoint) { // if we're at the end of the lane model path, we don't want to consider this a rejection if (laneModelClosestPoint < laneModelFarthestPoint) { // mark that at least on deviation was too high anyDeviationTooHigh = true; } break; } // increment the number of waypoint accepted numWaypointsAccepted++; // update the end point of where we're valid as the local road model was OK up to this point laneModelDeviationEndPoint = laneModelClosestPoint; } // go through and figure out how far out the variance is within tolerance LinePath.PointOnPath laneModelVarianceEndPoint = new LinePath.PointOnPath(); // walk forward from this point until the end of the lane mode path for (int i = laneModelZeroPoint.Index+1; i < laneModelPath.Count; i++) { // check if we're within the variance toleration if (laneModel.LaneYVariance[i] <= y_var_reject_threshold) { // we are, update the point on path laneModelVarianceEndPoint = laneModelPath.GetPointOnPath(i); } else { // we are out of tolerance, break out of the loop break; } } // now figure out everything out // determine waypoint rejection status WaypointRejectionResult waypointRejectionResult; if (laneModelDeviationEndPoint.Valid) { // if the point is valid, that we had at least one waypoint that was ok // check if any waypoints were rejected if (anyDeviationTooHigh) { // some waypoint was ok, so we know that at least one waypoint was accepted waypointRejectionResult = WaypointRejectionResult.SomeWaypointsAccepted; } else { // no waypoint triggered a rejection, but at least one was good waypointRejectionResult = WaypointRejectionResult.AllWaypointsAccepted; } } else { // the point is not valid, so we either had no waypoints or we had all rejections if (anyDeviationTooHigh) { // the first waypoint was rejected, so all are rejected waypointRejectionResult = WaypointRejectionResult.AllWaypointsRejected; } else { // the first waypoint (if any) was past the end of the lane model waypointRejectionResult = WaypointRejectionResult.NoWaypoints; } } // criteria for determining if this path is valid: // - if some or all waypoints were accepted, than this is probably a good path // - if some of the waypoints were accepted, we go no farther than the last waypoint that was accepted // - if there were no waypoints, this is a potentially dangerous situation since we can't reject // or confirm the local road model. for now, we'll assume that it is correct but this may need to change // if we start handling intersections in this framework // - if all waypoints were rejected, than we don't use the local road model // - go no farther than the laneModelVarianceEndPoint, which is the last point where the y-variance of // the lane model was in tolerance // now build out the lane model ILaneModel finalLaneModel; // check if we rejected all waypoints or no lane model points satisified the variance threshold if (waypointRejectionResult == WaypointRejectionResult.AllWaypointsRejected || !laneModelVarianceEndPoint.Valid) { // want to just use the path lane model finalLaneModel = new PathLaneModel(rndfPathTimestamp, rndfPath, rndfPathWidth); } else { // we'll use the lane model // need to build up the center line as well as left and right bounds // to build up the center line, use the lane model as far as we feel comfortable (limited by either variance // or by rejections) and then use the rndf lane after that. LinePath centerLine = new LinePath(); // figure out the max distance // if there were no waypoints, set the laneModelDeviationEndPoint to the end of the lane model if (waypointRejectionResult == WaypointRejectionResult.NoWaypoints) { laneModelDeviationEndPoint = laneModelFarthestPoint; } // figure out the closer of the end points LinePath.PointOnPath laneModelEndPoint = (laneModelDeviationEndPoint < laneModelVarianceEndPoint) ? laneModelDeviationEndPoint : laneModelVarianceEndPoint; bool endAtWaypoint = laneModelEndPoint == laneModelDeviationEndPoint; // add the lane model to the center line centerLine.AddRange(laneModelPath.GetSubpathEnumerator(laneModelZeroPoint, laneModelEndPoint)); // create a list to hold the width expansion values List<double> widthValue = new List<double>(); // make the width expansion values the width of the path plus the 1-sigma values for (int i = laneModelZeroPoint.Index; i < laneModelZeroPoint.Index+centerLine.Count; i++) { widthValue.Add(laneModel.Width/2.0 + Math.Sqrt(laneModel.LaneYVariance[i])); } // now figure out how to add the rndf path // get the projection of the lane model end point on the rndf path LinePath.PointOnPath rndfPathStartPoint = rndfPath.GetClosestPoint(laneModelEndPoint.Location); // if the closest point is past the end of rndf path, then we don't want to tack anything on if (rndfPathStartPoint != rndfPath.EndPoint) { // get the last segment of the new center line Coordinates centerLineEndSegmentVec = centerLine.EndSegment.UnitVector; // get the last point of the new center line Coordinates laneModelEndLoc = laneModelEndPoint.Location; // now figure out the distance to the next waypoint LinePath.PointOnPath rndfNextPoint = new LinePath.PointOnPath(); // figure out if we're ending at a waypoint or not if (endAtWaypoint) { rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index+1); // if the distance from the start point to the next point is less than rndf_dist_min, then // use the waypont after double dist = rndfPath.DistanceBetween(rndfPathStartPoint, rndfNextPoint); if (dist < rndf_dist_min) { if (rndfPathStartPoint.Index < rndfPath.Count-2) { rndfNextPoint = rndfPath.GetPointOnPath(rndfPathStartPoint.Index + 2); } else if (rndfPath.DistanceBetween(rndfPathStartPoint, rndfPath.EndPoint) < rndf_dist_min) { rndfNextPoint = LinePath.PointOnPath.Invalid; } else { rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, rndf_dist_min*2); } } } else { // track the last angle we had double lastAngle = double.NaN; // walk down the rndf path until we find a valid point for (double dist = rndf_dist_min; dist <= rndf_dist_max; dist += rndf_dist_step) { // advance from the start point by dist double distTemp = dist; rndfNextPoint = rndfPath.AdvancePoint(rndfPathStartPoint, ref distTemp); // if the distTemp is > 0, then we're past the end of the path if (distTemp > 0) { // if we're immediately past the end, we don't want to tack anything on if (dist == rndf_dist_min) { rndfNextPoint = LinePath.PointOnPath.Invalid; } break; } // check the angle made by the last segment of center line and the segment // formed between the end point of the center line and this new point double angle = Math.Acos(centerLineEndSegmentVec.Dot((rndfNextPoint.Location-laneModelEndLoc).Normalize())); // check if the angle satisfies the threshold or we're increasing the angle if (Math.Abs(angle) < rndf_angle_threshold || (!double.IsNaN(lastAngle) && angle > lastAngle)) { // break out of the loop, we're done searching break; } lastAngle = angle; } } // tack on the rndf starting at next point going to the end if (rndfNextPoint.Valid) { LinePath subPath = rndfPath.SubPath(rndfNextPoint, rndfPath.EndPoint); centerLine.AddRange(subPath); // insert the lane model end point into the sub path subPath.Insert(0, laneModelEndLoc); // get the angles List<Pair<int, double>> angles = subPath.GetIntersectionAngles(0, subPath.Count-1); // add the width of the path inflated by the angles for (int i = 0; i < angles.Count; i++) { // calculate the width expansion factor // 90 deg, 3x width // 45 deg, 1.5x width // 0 deg, 1x width double widthFactor = Math.Pow(angles[i].Right/(Math.PI/2.0), 2)*2 + 1; // add the width value widthValue.Add(widthFactor*laneModel.Width/2); } // add the final width widthValue.Add(laneModel.Width/2); } // set the rndf path start point to be the point we used rndfPathStartPoint = rndfNextPoint; } // for now, calculate the left and right bounds the same way we do for the path lane model // TODO: figure out if we want to do this more intelligently using knowledge of the lane model uncertainty LinePath leftBound = centerLine.ShiftLateral(widthValue.ToArray()); // get the next shifts for (int i = 0; i < widthValue.Count; i++) { widthValue[i] = -widthValue[i]; } LinePath rightBound = centerLine.ShiftLateral(widthValue.ToArray()); // build the final lane model finalLaneModel = new CombinedLaneModel(centerLine, leftBound, rightBound, laneModel.Width, rndfPathTimestamp); } SendLaneModelToUI(finalLaneModel, rndfPathTimestamp); // output the fit result return finalLaneModel; }
private PlanningPhase PlanStartingLane(LinePath curStartingLane, LinePath curEndingLane) { ILaneModel startingLaneModel = Services.RoadModelProvider.GetLaneModel(curStartingLane, startingLaneWidth + extraWidth, curTimestamp, startingLanesLeft, startingLanesRight); // figure out where we are along the starting lane path LinePath.PointOnPath startingLanePoint = curStartingLane.ZeroPoint; // calculate the planning distance double planningDistance = GetPlanningDistance(); if (sparse && planningDistance > 10) { planningDistance = 10; } // get the distance to the end point of the lane from our current point double remainingDistance = curStartingLane.DistanceBetween(startingLanePoint, curStartingLane.EndPoint); double? boundStartDistMin; double? boundEndDistMax; DetermineSparseStarting(planningDistance, out boundStartDistMin, out boundEndDistMax); double avoidanceExtra = sparse ? 5 : 7.5; // linearize the lane model LinePath centerLine, leftBound, rightBound; if (boundEndDistMax != null || boundStartDistMin != null) { if (boundEndDistMax != null) { boundEndDistMax = Math.Min(boundEndDistMax.Value, remainingDistance - starting_lane_trim_dist); } else { boundEndDistMax = remainingDistance - starting_lane_trim_dist; } LinearizeStayInLane(startingLaneModel, planningDistance + avoidanceExtra, null, boundEndDistMax, boundStartDistMin, null, curTimestamp, out centerLine, out leftBound, out rightBound); } else { LinearizeStayInLane(startingLaneModel, planningDistance + avoidanceExtra, null, remainingDistance - starting_lane_trim_dist, null, curTimestamp, out centerLine, out leftBound, out rightBound); } BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in stay in lane, center line count: {0}, left bound count: {1}, right bound count: {2}", centerLine.Count, leftBound.Count, rightBound.Count); // add to the planning setup avoidanceBasePath = centerLine; double targetDist = Math.Max(centerLine.PathLength-avoidanceExtra, planningDistance); smootherBasePath = centerLine.SubPath(centerLine.StartPoint, targetDist); // calculate time to collision of opposing obstacle if it exists LinePath targetLine = centerLine; double targetWeight = default_lane_alpha_w; if (sparse) { targetWeight = 0.00000025; } else if (oncomingVehicleExists) { double shiftDist = -(TahoeParams.T + 1); targetLine = centerLine.ShiftLateral(shiftDist); targetWeight = 0.01; } else if (opposingLaneVehicleExists) { double timeToCollision = opposingLaneVehicleDist/(Math.Abs(vs.speed)+Math.Abs(opposingLaneVehicleSpeed)); Services.Dataset.ItemAs<double>("time to collision").Add(timeToCollision, curTimestamp); if (timeToCollision < 5) { // shift to the right double laneWidth = startingLaneModel.Width; double shiftDist = -Math.Min(TahoeParams.T/2.0, laneWidth/2.0 - TahoeParams.T/2.0); targetLine = centerLine.ShiftLateral(shiftDist); } } // set up the planning AddTargetPath(targetLine, targetWeight); if (!sparse || boundStartDistMin != null || boundEndDistMax != null) { // add the lane bounds AddLeftBound(leftBound, true); if (!oncomingVehicleExists) { AddRightBound(rightBound, false); } } Services.UIService.PushLineList(centerLine, curTimestamp, "subpath", true); Services.UIService.PushLineList(leftBound, curTimestamp, "left bound", true); Services.UIService.PushLineList(rightBound, curTimestamp, "right bound", true); // calculate max speed for the path over the next 120 meters double advanceDist = speed_path_advance_dist; LinePath.PointOnPath startingLaneEndPoint = curStartingLane.AdvancePoint(startingLanePoint, ref advanceDist); LinePath combinedPath = curStartingLane.SubPath(startingLanePoint, startingLaneEndPoint); Coordinates endingLaneFirstPoint = curEndingLane[0]; Coordinates startingLaneLastPoint = curStartingLane.EndPoint.Location; double intersectionDist = startingLaneLastPoint.DistanceTo(endingLaneFirstPoint); // add a portion of the ending lane of the appropriate length LinePath.PointOnPath endingLanePoint = curEndingLane.AdvancePoint(curEndingLane.StartPoint, Math.Max(advanceDist - intersectionDist, 5)); int endIndex = endingLanePoint.Index + 1; combinedPath.AddRange(curEndingLane.GetSubpathEnumerator(0, endIndex)); // add to the planning setup settings.maxSpeed = GetMaxSpeed(combinedPath, combinedPath.StartPoint); settings.maxEndingSpeed = GetMaxSpeed(combinedPath, combinedPath.AdvancePoint(combinedPath.StartPoint, planningDistance)); if (sparse) { // limit to 5 mph laneWidthAtPathEnd = 20; pathAngleCheckMax = 50; pathAngleMax = 5 * Math.PI / 180.0; settings.maxSpeed = Math.Min(settings.maxSpeed, 2.2352); LinePath leftEdge = RoadEdge.GetLeftEdgeLine(); LinePath rightEdge = RoadEdge.GetRightEdgeLine(); if (leftEdge != null) { leftLaneBounds.Add(new Boundary(leftEdge, 0.1, 1, 100, false)); } if (rightEdge != null) { rightLaneBounds.Add(new Boundary(rightEdge, 0.1, 1, 100, false)); } maxAvoidanceBasePathAdvancePoint = avoidanceBasePath.AdvancePoint(avoidanceBasePath.EndPoint, -2); //maxSmootherBasePathAdvancePoint = smootherBasePath.AdvancePoint(smootherBasePath.EndPoint, -2); PlanningResult result = new PlanningResult(); ISteeringCommandGenerator commandGenerator = SparseArcVoting.SparcVote(ref prevCurvature, goalPoint); if (commandGenerator == null) { // we have a block, report dynamically infeasible result = OnDynamicallyInfeasible(null, null); } else { result.steeringCommandGenerator = commandGenerator; result.commandLabel = commandLabel; } Track(result, commandLabel); if (planningDistance > remainingDistance) { return PlanningPhase.BeginEnteringTurn; } else { return PlanningPhase.StartingLane; } } else if (oncomingVehicleExists) { laneWidthAtPathEnd = 10; } BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "max speed set to {0}", settings.maxSpeed); SmoothAndTrack(commandLabel, true); if (cancelled) return PlanningPhase.StartingLane; // check if we need to transition to the turn phase for the next iteration // if the planning distance is greater than the remaining distance, i.e. we want to plan into the intersection, then // we want to partially run the turn behavior next iteration if (planningDistance > remainingDistance) { return PlanningPhase.BeginEnteringTurn; } else { return PlanningPhase.StartingLane; } }
private PlanningPhase PlanEnteringTurn(LinePath curStartingLane, LinePath curEndingLane) { // figure out where we are along the starting lane path LinePath.PointOnPath startingLanePoint = curStartingLane.ZeroPoint; // get the distance to the end point of the lane from our current point double remainingDistance = curStartingLane.DistanceBetween(startingLanePoint, curStartingLane.EndPoint); // immediately switch to the mid turn mode if the remaining distance is less than the front-length // this prevents problems with the lane linearization extending out past the end of the lane if (remainingDistance < TahoeParams.FL) { return PlanMidTurn(curStartingLane, curEndingLane); } // get the lane mode for the starting lane ILaneModel startingLaneModel = Services.RoadModelProvider.GetLaneModel(curStartingLane, startingLaneWidth + extraWidth, curTimestamp, startingLanesLeft, startingLanesRight); double? boundStartDistMin; double? boundEndDistMax; DetermineSparseStarting(remainingDistance - starting_lane_trim_dist, out boundStartDistMin, out boundEndDistMax); // linearize the lane model LinePath centerLine, leftBound, rightBound; if (boundEndDistMax != null || boundStartDistMin != null) { if (boundEndDistMax != null) { boundEndDistMax = Math.Min(boundEndDistMax.Value, remainingDistance - starting_lane_trim_dist); } else { boundEndDistMax = remainingDistance - starting_lane_trim_dist; } LinearizeStayInLane(startingLaneModel, remainingDistance, 0, boundEndDistMax, boundStartDistMin, null, curTimestamp, out centerLine, out leftBound, out rightBound); } else { LinearizeStayInLane(startingLaneModel, remainingDistance, 0, remainingDistance - starting_lane_trim_dist, null, curTimestamp, out centerLine, out leftBound, out rightBound); } // create a copy so when we add to the center line later we don't change this version LinePath startingCenterLine = centerLine.Clone(); // calculate time to collision of opposing obstacle if it exists LinePath targetLine = startingCenterLine; double targetWeight = default_lane_alpha_w; if (oncomingVehicleExists) { double shiftDist = -(TahoeParams.T + 1); targetLine = centerLine.ShiftLateral(shiftDist); targetWeight = 0.01; } if (opposingLaneVehicleExists) { double timeToCollision = opposingLaneVehicleDist/(Math.Abs(vs.speed)+Math.Abs(opposingLaneVehicleSpeed)); Services.Dataset.ItemAs<double>("time to collision").Add(timeToCollision, curTimestamp); if (timeToCollision < 5) { // shift to the right double laneWidth = startingLaneModel.Width; double shiftDist = -Math.Min(TahoeParams.T/2.0, laneWidth/2.0 - TahoeParams.T/2.0); targetLine = centerLine.ShiftLateral(shiftDist); } } // set up the planning AddTargetPath(targetLine, targetWeight); // add the lane bounds if (!sparse || boundStartDistMin != null || boundEndDistMax != null) { // add the lane bounds AddLeftBound(leftBound, true); if (!oncomingVehicleExists) { AddRightBound(rightBound, false); } } LinePath endingCenterLine, endingLeftBound, endingRightBound; LinearizeLane(curEndingLane, endingLaneWidth, 0, 5, 2, 6, out endingCenterLine, out endingLeftBound, out endingRightBound); // add the ending lane center line to the target paths AddTargetPath(endingCenterLine, 0.001); // add the lane bounds AddLeftBound(endingLeftBound, false); AddRightBound(endingRightBound, false); // intert a small portion of the ending lane centerLine.AddRange(endingCenterLine); // get the intersection pull path LinePath intersectionPullPath = new LinePath(); double pullWeighting = 0; GetIntersectionPullPath(startingCenterLine, endingCenterLine, intersectionPolygon, true, true, intersectionPullPath, ref pullWeighting); if (intersectionPullPath.Count > 0) { // add as a weighting term AddTargetPath(intersectionPullPath, pullWeighting); } BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in stay in lane, center line count: {0}, left bound count: {1}, right bound count: {2}", centerLine.Count, leftBound.Count, rightBound.Count); Services.UIService.PushLineList(centerLine, curTimestamp, "subpath", true); Services.UIService.PushLineList(leftBound, curTimestamp, "left bound", true); Services.UIService.PushLineList(rightBound, curTimestamp, "right bound", true); // calculate max speed for the path over the next 120 meters double advanceDist = speed_path_advance_dist; LinePath.PointOnPath startingLaneEndPoint = curStartingLane.AdvancePoint(startingLanePoint, ref advanceDist); LinePath combinedPath = curStartingLane.SubPath(startingLanePoint, startingLaneEndPoint); if (advanceDist > 0) { Coordinates endingLaneFirstPoint = curEndingLane[0]; Coordinates startingLaneLastPoint = curStartingLane.EndPoint.Location; double intersectionDist = startingLaneLastPoint.DistanceTo(endingLaneFirstPoint); double addDist = Math.Max(advanceDist - intersectionDist, 5); // add a portion of the ending lane of the appropriate length LinePath.PointOnPath endingLanePoint = curEndingLane.AdvancePoint(curEndingLane.StartPoint, addDist); int endIndex = endingLanePoint.Index + 1; combinedPath.AddRange(curEndingLane.GetSubpathEnumerator(0, endIndex)); } settings.maxSpeed = GetMaxSpeed(combinedPath, combinedPath.StartPoint); if (sparse) { // limit to 5 mph //laneWidthAtPathEnd = 20; settings.maxSpeed = Math.Min(settings.maxSpeed, 2.2352); LinePath leftEdge = RoadEdge.GetLeftEdgeLine(); LinePath rightEdge = RoadEdge.GetRightEdgeLine(); if (leftEdge != null) { additionalLeftBounds.Add(new Boundary(leftEdge, 0.1, 1, 100, false)); } if (rightEdge != null) { additionalRightBounds.Add(new Boundary(rightEdge, 0.1, 1, 100, false)); } } BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "max speed set to {0}", settings.maxSpeed); // set the avoidance path to be the previously smoothed path AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp); avoidanceBasePath = turnBasePath.Transform(absTransform); avoidanceBasePath = avoidanceBasePath.SubPath(avoidanceBasePath.ZeroPoint, avoidanceBasePath.EndPoint); smootherBasePath = avoidanceBasePath.Clone(); maxSmootherBasePathAdvancePoint = smootherBasePath.GetClosestPoint(startingCenterLine.EndPoint.Location); // set auxillary settings settings.endingHeading = centerLine.EndSegment.UnitVector.ArcTan; //useAvoidancePath = true; SmoothAndTrack(commandLabel, true); if (cancelled) return PlanningPhase.EnteringTurn; // check if the remaining distance on the starting lane is less than some small tolerance. // if it is, switch to the mid-turn phase if (remainingDistance <= TahoeParams.FL) { return PlanningPhase.MidTurn; } else { return PlanningPhase.EnteringTurn; } }
private PlanningPhase PlanBeginningTurn(LinePath curStartingLane, LinePath curEndingLane) { // figure out where we are along the starting lane path LinePath.PointOnPath startingLanePoint = curStartingLane.ZeroPoint; // get the distance to the end point of the lane from our current point double remainingDistance = curStartingLane.DistanceBetween(startingLanePoint, curStartingLane.EndPoint); // get the lane mode for the starting lane ILaneModel startingLaneModel = Services.RoadModelProvider.GetLaneModel(curStartingLane, startingLaneWidth + extraWidth, curTimestamp, startingLanesLeft, startingLanesRight); double? boundStartDistMin; double? boundEndDistMax; DetermineSparseStarting(remainingDistance - starting_lane_trim_dist, out boundStartDistMin, out boundEndDistMax); // linearize the lane model LinePath centerLine, leftBound, rightBound; if (boundEndDistMax != null || boundStartDistMin != null) { if (boundEndDistMax != null) { boundEndDistMax = Math.Min(boundEndDistMax.Value, remainingDistance - starting_lane_trim_dist); } else { boundEndDistMax = remainingDistance - starting_lane_trim_dist; } LinearizeStayInLane(startingLaneModel, remainingDistance, 0, boundEndDistMax, boundStartDistMin, null, curTimestamp, out centerLine, out leftBound, out rightBound); } else { LinearizeStayInLane(startingLaneModel, remainingDistance, 0, remainingDistance - starting_lane_trim_dist, null, curTimestamp, out centerLine, out leftBound, out rightBound); } //LinearizeLane(curStartingLane, startingLaneWidth, TahoeParams.FL, remainingDistance, 5, remainingDistance - 5, out centerLine, out leftBound, out rightBound); // create a copy so when we add to the center line later we don't change this version LinePath startingCenterLine = centerLine.Clone(); // calculate time to collision of opposing obstacle if it exists LinePath targetLine = startingCenterLine; if (opposingLaneVehicleExists) { double timeToCollision = opposingLaneVehicleDist/(Math.Abs(vs.speed)+Math.Abs(opposingLaneVehicleSpeed)); if (timeToCollision < 3) { // shift to the right double shiftDist = -TahoeParams.T/2.0; targetLine = centerLine.ShiftLateral(shiftDist); } } // set up the planning AddTargetPath(targetLine, default_lane_alpha_w); if (!sparse || boundStartDistMin != null || boundEndDistMax != null) { // add the lane bounds AddLeftBound(leftBound, true); if (!oncomingVehicleExists) { AddRightBound(rightBound, false); } } LinePath endingCenterLine, endingLeftBound, endingRightBound; LinearizeLane(curEndingLane, endingLaneWidth, 0, 5, 2, 6, out endingCenterLine, out endingLeftBound, out endingRightBound); // add the ending lane center line to the target paths AddTargetPath(endingCenterLine, default_lane_alpha_w); // add the lane bounds AddLeftBound(endingLeftBound, false); AddRightBound(endingRightBound, false); // intert a small portion of the ending lane centerLine.AddRange(endingCenterLine); // add to the planning setup smootherBasePath = centerLine; maxSmootherBasePathAdvancePoint = smootherBasePath.GetClosestPoint(startingCenterLine.EndPoint.Location); // get the intersection pull path LinePath intersectionPullPath = new LinePath(); double pullWeighting = 0; GetIntersectionPullPath(startingCenterLine, endingCenterLine, intersectionPolygon, true, true, intersectionPullPath, ref pullWeighting); if (intersectionPullPath.Count > 0) { // add as a weighting term AddTargetPath(intersectionPullPath, pullWeighting); } BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in stay in lane, center line count: {0}, left bound count: {1}, right bound count: {2}", centerLine.Count, leftBound.Count, rightBound.Count); // calculate max speed for the path over the next 120 meters double advanceDist = speed_path_advance_dist; LinePath.PointOnPath startingLaneEndPoint = curStartingLane.AdvancePoint(startingLanePoint, ref advanceDist); LinePath combinedPath = curStartingLane.SubPath(startingLanePoint, startingLaneEndPoint); if (advanceDist > 0) { Coordinates endingLaneFirstPoint = curEndingLane[0]; Coordinates startingLaneLastPoint = curStartingLane.EndPoint.Location; double intersectionDist = startingLaneLastPoint.DistanceTo(endingLaneFirstPoint); double addDist = Math.Max(advanceDist - intersectionDist, 5); // add a portion of the ending lane of the appropriate length LinePath.PointOnPath endingLanePoint = curEndingLane.AdvancePoint(curEndingLane.StartPoint, addDist); int endIndex = endingLanePoint.Index + 1; combinedPath.AddRange(curEndingLane.GetSubpathEnumerator(0, endIndex)); } // set the planning settings max speed settings.maxSpeed = GetMaxSpeed(combinedPath, combinedPath.StartPoint); if (sparse) { // limit to 5 mph //laneWidthAtPathEnd = 20; settings.maxSpeed = Math.Min(settings.maxSpeed, 2.2352); } BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "max speed set to {0}", settings.maxSpeed); // set auxillary settings settings.endingHeading = centerLine.EndSegment.UnitVector.ArcTan; PlanningResult planningResult = Smooth(false); if (!planningResult.pathBlocked && !planningResult.dynamicallyInfeasible) { AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp); turnBasePath = planningResult.smoothedPath.Transform(absTransform.Invert()); return PlanningPhase.EnteringTurn; } else { // keep trying I guess return PlanningPhase.BeginEnteringTurn; } }
/// <summary> /// Gets primary maneuver given our position and the turn we are traveling upon /// </summary> /// <param name="vehicleState"></param> /// <returns></returns> public Maneuver PrimaryManeuver(VehicleState vehicleState, List <ITacticalBlockage> blockages, TurnState turnState) { #region Check are planning over the correct turn if (CoreCommon.CorePlanningState is TurnState) { TurnState ts = (TurnState)CoreCommon.CorePlanningState; if (this.turn == null || !this.turn.Equals(ts.Interconnect)) { this.turn = ts.Interconnect; this.forwardMonitor = new TurnForwardMonitor(ts.Interconnect, null); } else if (this.forwardMonitor.turn == null || !this.forwardMonitor.turn.Equals(ts.Interconnect)) { this.forwardMonitor = new TurnForwardMonitor(ts.Interconnect, null); } } #endregion #region Blockages // check blockages if (blockages != null && blockages.Count > 0 && blockages[0] is TurnBlockage) { // create the blockage state EncounteredBlockageState ebs = new EncounteredBlockageState(blockages[0], CoreCommon.CorePlanningState); // check not at highest level already if (turnState.Saudi != SAUDILevel.L1 || turnState.UseTurnBounds) { // check not from a dynamicly moving vehicle if (blockages[0].BlockageReport.BlockageType != BlockageType.Dynamic || (TacticalDirector.ValidVehicles.ContainsKey(blockages[0].BlockageReport.TrackID) && TacticalDirector.ValidVehicles[blockages[0].BlockageReport.TrackID].IsStopped)) { // go to a blockage handling tactical return(new Maneuver(new NullBehavior(), ebs, TurnDecorators.NoDecorators, vehicleState.Timestamp)); } else { ArbiterOutput.Output("Turn blockage reported for moving vehicle, ignoring"); } } else { ArbiterOutput.Output("Turn blockage, but recovery escalation already at highest state, ignoring report"); } } #endregion #region Intersection Check if (!this.CanGo(vehicleState)) { if (turn.FinalGeneric is ArbiterWaypoint) { TravelingParameters tp = this.GetParameters(0.0, 0.0, (ArbiterWaypoint)turn.FinalGeneric, vehicleState, false); return(new Maneuver(tp.Behavior, CoreCommon.CorePlanningState, tp.NextState.DefaultStateDecorators, vehicleState.Timestamp)); } else { // get turn params LinePath finalPath; LineList leftLL; LineList rightLL; IntersectionToolkit.ZoneTurnInfo(this.turn, (ArbiterPerimeterWaypoint)this.turn.FinalGeneric, out finalPath, out leftLL, out rightLL); // hold brake IState nextState = new TurnState(this.turn, this.turn.TurnDirection, null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(0.0)); TurnBehavior b = new TurnBehavior(null, finalPath, leftLL, rightLL, new ScalarSpeedCommand(0.0), this.turn.InterconnectId); return(new Maneuver(b, CoreCommon.CorePlanningState, nextState.DefaultStateDecorators, vehicleState.Timestamp)); } } #endregion #region Final is Lane Waypoint if (turn.FinalGeneric is ArbiterWaypoint) { // final point ArbiterWaypoint final = (ArbiterWaypoint)turn.FinalGeneric; // plan down entry lane RoadPlan rp = navigation.PlanNavigableArea(final.Lane, final.Position, CoreCommon.RoadNetwork.ArbiterWaypoints[CoreCommon.Mission.MissionCheckpoints.Peek().WaypointId], new List <ArbiterWaypoint>()); // point of interest downstream DownstreamPointOfInterest dpoi = rp.BestPlan.laneWaypointOfInterest; // get path this represents List <Coordinates> pathCoordinates = new List <Coordinates>(); pathCoordinates.Add(vehicleState.Position); foreach (ArbiterWaypoint aw in final.Lane.WaypointsInclusive(final, final.Lane.WaypointList[final.Lane.WaypointList.Count - 1])) { pathCoordinates.Add(aw.Position); } LinePath lp = new LinePath(pathCoordinates); // list of all parameterizations List <TravelingParameters> parameterizations = new List <TravelingParameters>(); // get lane navigation parameterization TravelingParameters navigationParameters = this.NavigationParameterization(vehicleState, dpoi, final, lp); parameterizations.Add(navigationParameters); // update forward tracker and get vehicle parameterizations if forward vehicle exists this.forwardMonitor.Update(vehicleState, final, lp); if (this.forwardMonitor.ShouldUseForwardTracker()) { // get vehicle parameterization TravelingParameters vehicleParameters = this.VehicleParameterization(vehicleState, lp, final); parameterizations.Add(vehicleParameters); } // sort and return funal parameterizations.Sort(); // get the final behavior TurnBehavior tb = (TurnBehavior)parameterizations[0].Behavior; // get vehicles to ignore tb.VehiclesToIgnore = this.forwardMonitor.VehiclesToIgnore; // add persistent information about saudi level if (turnState.Saudi == SAUDILevel.L1) { tb.Decorators = new List <BehaviorDecorator>(tb.Decorators.ToArray()); tb.Decorators.Add(new ShutUpAndDoItDecorator(SAUDILevel.L1)); } // add persistent information about turn bounds if (!turnState.UseTurnBounds) { tb.LeftBound = null; tb.RightBound = null; } // return the behavior return(new Maneuver(tb, CoreCommon.CorePlanningState, tb.Decorators, vehicleState.Timestamp)); } #endregion #region Final is Zone Waypoint else if (turn.FinalGeneric is ArbiterPerimeterWaypoint) { // get inteconnect path Coordinates entryVec = ((ArbiterPerimeterWaypoint)turn.FinalGeneric).Perimeter.PerimeterPolygon.BoundingCircle.center - turn.FinalGeneric.Position; entryVec = entryVec.Normalize(TahoeParams.VL / 2.0); LinePath ip = new LinePath(new Coordinates[] { turn.InitialGeneric.Position, turn.FinalGeneric.Position, entryVec + this.turn.FinalGeneric.Position }); // get distance from end double d = ip.DistanceBetween( ip.GetClosestPoint(vehicleState.Front), ip.EndPoint); // get speed command SpeedCommand sc = null; if (d < TahoeParams.VL) { sc = new StopAtDistSpeedCommand(d); } else { sc = new ScalarSpeedCommand(SpeedTools.GenerateSpeed(d - TahoeParams.VL, 1.7, turn.MaximumDefaultSpeed)); } // final perimeter waypoint ArbiterPerimeterWaypoint apw = (ArbiterPerimeterWaypoint)this.turn.FinalGeneric; // get turn params LinePath finalPath; LineList leftLL; LineList rightLL; IntersectionToolkit.ZoneTurnInfo(this.turn, (ArbiterPerimeterWaypoint)this.turn.FinalGeneric, out finalPath, out leftLL, out rightLL); // hold brake IState nextState = new TurnState(this.turn, this.turn.TurnDirection, null, finalPath, leftLL, rightLL, sc); TurnBehavior tb = new TurnBehavior(null, finalPath, leftLL, rightLL, sc, null, new List <int>(), this.turn.InterconnectId); // add persistent information about saudi level if (turnState.Saudi == SAUDILevel.L1) { tb.Decorators = new List <BehaviorDecorator>(tb.Decorators.ToArray()); tb.Decorators.Add(new ShutUpAndDoItDecorator(SAUDILevel.L1)); } // add persistent information about turn bounds if (!turnState.UseTurnBounds) { tb.LeftBound = null; tb.RightBound = null; } // return maneuver return(new Maneuver(tb, CoreCommon.CorePlanningState, tb.Decorators, vehicleState.Timestamp)); } #endregion #region Unknown else { throw new Exception("unrecognized type: " + turn.FinalGeneric.ToString()); } #endregion }
/// <summary> /// Check if we can go /// </summary> /// <param name="vs"></param> private bool CanGo(VehicleState vs) { #region Moving Vehicles Inside Turn // check if we can still go through this turn if (TacticalDirector.VehicleAreas.ContainsKey(this.turn)) { // get the subpath of the interconnect we care about LinePath.PointOnPath frontPos = this.turn.InterconnectPath.GetClosestPoint(vs.Front); LinePath aiSubpath = this.turn.InterconnectPath.SubPath(frontPos, this.turn.InterconnectPath.EndPoint); if (aiSubpath.PathLength > 4.0) { aiSubpath = aiSubpath.SubPath(aiSubpath.StartPoint, aiSubpath.PathLength - 2.0); // get vehicles List <VehicleAgent> turnVehicles = TacticalDirector.VehicleAreas[this.turn]; // loop vehicles foreach (VehicleAgent va in turnVehicles) { // check if inside turn LinePath.PointOnPath vaPop = aiSubpath.GetClosestPoint(va.ClosestPosition); if (!va.IsStopped && this.turn.TurnPolygon.IsInside(va.ClosestPosition) && !vaPop.Equals(aiSubpath.StartPoint) && !vaPop.Equals(aiSubpath.EndPoint)) { ArbiterOutput.Output("Vehicle seen inside our turn: " + va.ToString() + ", stopping"); return(false); } } } } #endregion // test if this turn is part of an intersection if (CoreCommon.RoadNetwork.IntersectionLookup.ContainsKey(this.turn.InitialGeneric.AreaSubtypeWaypointId)) { // intersection ArbiterIntersection inter = CoreCommon.RoadNetwork.IntersectionLookup[this.turn.InitialGeneric.AreaSubtypeWaypointId]; // check if priority lanes exist for this interconnect if (inter.PriorityLanes.ContainsKey(this.turn)) { // get all the default priority lanes List <IntersectionInvolved> priorities = inter.PriorityLanes[this.turn]; // get the subpath of the interconnect we care about //LinePath.PointOnPath frontPos = this.turn.InterconnectPath.GetClosestPoint(vs.Front); LinePath aiSubpath = new LinePath(new List <Coordinates>(new Coordinates[] { vs.Front, this.turn.FinalGeneric.Position })); //this.turn.InterconnectPath.SubPath(frontPos, this.turn.InterconnectPath.EndPoint); // check if path ended if (aiSubpath.Count < 2) { return(true); } // determine all of the new priority lanes List <IntersectionInvolved> updatedPriorities = new List <IntersectionInvolved>(); #region Determine new priority areas given position // loop through old priorities foreach (IntersectionInvolved ii in priorities) { // check ii lane if (ii.Area is ArbiterLane) { #region Lane Intersects Turn Path w/ Point of No Return // check if the waypoint is not the last waypoint in the lane if (ii.Exit == null || ((ArbiterWaypoint)ii.Exit).NextPartition != null) { // check where line intersects path Coordinates?intersect = this.LaneIntersectsPath(ii, aiSubpath, this.turn.FinalGeneric); // check for an intersection if (intersect.HasValue) { // distance to intersection double distanceToIntersection = (intersect.Value.DistanceTo(vs.Front) + ((ArbiterLane)ii.Area).LanePath().GetClosestPoint(vs.Front).Location.DistanceTo(vs.Front)) / 2.0; // determine if we can stop before or after the intersection double distanceToStoppage = RoadToolkit.DistanceToStop(CoreCommon.Communications.GetVehicleSpeed().Value); // check dist to intersection > distance to stoppage if (distanceToIntersection > distanceToStoppage) { // add updated priority updatedPriorities.Add(new IntersectionInvolved(new ArbiterWaypoint(intersect.Value, new ArbiterWaypointId(0, ((ArbiterLane)ii.Area).LaneId)), ii.Area, ArbiterTurnDirection.Straight)); } else { ArbiterOutput.Output("Passed point of No Return for Lane: " + ii.Area.ToString()); } } } #endregion // we know there is an exit and it is the last waypoint of the segment, fuxk! else { #region Turns Intersect // get point to look at if exists ArbiterInterconnect interconnect; Coordinates? intersect = this.TurnIntersects(aiSubpath, ii.Exit, out interconnect); // check for the intersect if (intersect.HasValue) { ArbiterLane al = (ArbiterLane)ii.Area; LinePath lp = al.LanePath().SubPath(al.LanePath().StartPoint, al.LanePath().GetClosestPoint(ii.Exit.Position)); lp.Add(interconnect.InterconnectPath.EndPoint.Location); // get our time to the intersection point //double ourTime = Math.Min(4.0, Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.001 ? aiSubpath.PathLength / 1.0 : aiSubpath.PathLength / Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value)); // get our time to the intersection point double ourSpeed = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value); double stoppedTime = ourSpeed < 1.0 ? 1.5 : 0.0; double extraTime = 1.5; double interconnectTime = aiSubpath.PathLength / this.turn.MaximumDefaultSpeed; double ourTime = Math.Min(6.5, stoppedTime + extraTime + interconnectTime); // get closest vehicle in that lane to the intersection List <VehicleAgent> toLook = new List <VehicleAgent>(); if (TacticalDirector.VehicleAreas.ContainsKey(ii.Area)) { foreach (VehicleAgent tmpVa in TacticalDirector.VehicleAreas[ii.Area]) { double upstreamDist = al.DistanceBetween(tmpVa.ClosestPosition, ii.Exit.Position); if (upstreamDist > 0 && tmpVa.PassedDelayedBirth) { toLook.Add(tmpVa); } } } if (TacticalDirector.VehicleAreas.ContainsKey(interconnect)) { toLook.AddRange(TacticalDirector.VehicleAreas[interconnect]); } // check length of stuff to look at if (toLook.Count > 0) { foreach (VehicleAgent va in toLook) { // distance along path to location of intersect double distToIntersect = lp.DistanceBetween(lp.GetClosestPoint(va.ClosestPosition), lp.GetClosestPoint(aiSubpath.GetClosestPoint(va.ClosestPosition).Location)); double speed = va.Speed == 0.0 ? 0.01 : va.Speed; double vaTime = distToIntersect / Math.Abs(speed); if (vaTime > 0 && vaTime < ourTime) { ArbiterOutput.Output("va: " + va.ToString() + " CollisionTimer: " + vaTime.ToString("f2") + " < TimeUs: " + ourTime.ToString("F2") + ", NOGO"); return(false); } } } } #endregion } } } #endregion #region Updated Priority Intersection Code // loop through updated priorities bool updatedPrioritiesClear = true; foreach (IntersectionInvolved ii in updatedPriorities) { // lane ArbiterLane al = (ArbiterLane)ii.Area; // get our time to the intersection point double ourSpeed = Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value); double stoppedTime = ourSpeed < 1.0 ? 1.5 : 0.0; double extraTime = 1.5; double interconnectTime = aiSubpath.PathLength / this.turn.MaximumDefaultSpeed; double ourTime = Math.Min(6.5, stoppedTime + extraTime + interconnectTime); // double outTime = Math.Min(4.0, Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value) < 0.001 ? aiSubpath.PathLength / 1.0 : aiSubpath.PathLength / Math.Abs(CoreCommon.Communications.GetVehicleSpeed().Value)); // get closest vehicle in that lane to the intersection if (TacticalDirector.VehicleAreas.ContainsKey(ii.Area)) { // get lane vehicles List <VehicleAgent> vas = TacticalDirector.VehicleAreas[ii.Area]; // determine for all VehicleAgent closestLaneVa = null; double closestDistanceVa = Double.MaxValue; double closestTime = Double.MaxValue; foreach (VehicleAgent testVa in vas) { // check upstream double distance = al.DistanceBetween(testVa.ClosestPosition, ii.Exit.Position); // get speed double speed = testVa.Speed; double time = testVa.StateMonitor.Observed.speedValid ? distance / Math.Abs(speed) : distance / al.Way.Segment.SpeedLimits.MaximumSpeed; // check distance > 0 if (distance > 0) { // check if closer or none other exists if (closestLaneVa == null || time < closestTime) { closestLaneVa = testVa; closestDistanceVa = distance; closestTime = time; } } } // check if closest exists if (closestLaneVa != null) { // set va VehicleAgent va = closestLaneVa; double distance = closestDistanceVa; // check dist and birth time if (distance > 0 && va.PassedDelayedBirth) { // check time double speed = va.Speed == 0.0 ? 0.01 : va.Speed; double time = va.StateMonitor.Observed.speedValid ? distance / Math.Abs(speed) : distance / al.Way.Segment.SpeedLimits.MaximumSpeed; // too close if (!al.LanePolygon.IsInside(CoreCommon.Communications.GetVehicleState().Front) && distance < 25 && (!va.StateMonitor.Observed.speedValid || !va.StateMonitor.Observed.isStopped) && CoreCommon.Communications.GetVehicleState().Front.DistanceTo(va.ClosestPosition) < 20) { ArbiterOutput.Output("Turn, NOGO, Lane: " + al.ToString() + " vehicle: " + va.ToString() + " possibly moving to close, stopping"); //return false; updatedPrioritiesClear = false; return(false); } else if (time > 0 && time < ourTime) { ArbiterOutput.Output("Turn, NOGO, Lane: " + al.ToString() + ", va: " + va.ToString() + ", stopped: " + va.IsStopped.ToString() + ", timeUs: " + ourTime.ToString("f2") + ", timeThem: " + time.ToString("f2")); //return false; updatedPrioritiesClear = false; return(false); } else { ArbiterOutput.Output("Turn, CANGO, Lane: " + al.ToString() + ", va: " + va.ToString() + ", stopped: " + va.IsStopped.ToString() + ", timeUs: " + ourTime.ToString("f2") + ", timeThem: " + time.ToString("f2")); //return true; } } } else { ArbiterOutput.Output("Turn, CANGO, Lane: " + al.ToString() + " has no traffic vehicles"); } } } return(updatedPrioritiesClear); #endregion } } // fall through fine to go ArbiterOutput.Output("In Turn, CAN GO, Clear of vehicles upstream"); return(true); }
public override void Process(object param) { // inform the base that we're beginning processing if (!BeginProcess()) { return; } // check if we were given a parameter if (param != null && param is StayInLaneBehavior) { HandleBehavior((StayInLaneBehavior)param); } if (reverseGear) { ProcessReverse(); return; } // figure out the planning distance double planningDistance = GetPlanningDistance(); if (sparse && planningDistance > 10) { planningDistance = 10; } double?boundStartDistMin = null; double?boundEndDistMax = null; if (laneID != null && Services.RoadNetwork != null) { ArbiterLane lane = Services.RoadNetwork.ArbiterSegments[laneID.SegmentId].Lanes[laneID]; AbsolutePose pose = Services.StateProvider.GetAbsolutePose(); ArbiterLanePartition partition = lane.GetClosestPartition(pose.xy); LinePath partitionPath = partition.UserPartitionPath; LinePath.PointOnPath closestPoint = partitionPath.GetClosestPoint(pose.xy); double remainingDist = planningDistance; double totalDist = partitionPath.DistanceBetween(closestPoint, partitionPath.EndPoint); remainingDist -= totalDist; if (sparse) { // walk ahead and determine where sparsity ends bool nonSparseFound = false; while (remainingDist > 0) { // get the next partition partition = partition.Final.NextPartition; if (partition == null) { break; } if (partition.Type != PartitionType.Sparse) { nonSparseFound = true; break; } else { double dist = partition.UserPartitionPath.PathLength; totalDist += dist; remainingDist -= dist; } } if (nonSparseFound) { boundStartDistMin = totalDist; } } else { // determine if there is a sparse segment upcoming bool sparseFound = false; while (remainingDist > 0) { partition = partition.Final.NextPartition; if (partition == null) { break; } if (partition.Type == PartitionType.Sparse) { sparseFound = true; break; } else { double dist = partition.Length; totalDist += dist; remainingDist -= dist; } } if (sparseFound) { boundEndDistMax = totalDist; sparse = true; } } } BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "in stay in lane, planning distance {0}", planningDistance); // update the rndf path RelativeTransform relTransfrom = Services.RelativePose.GetTransform(behaviorTimestamp, curTimestamp); LinePath curRndfPath = rndfPath.Transform(relTransfrom); ILaneModel centerLaneModel = Services.RoadModelProvider.GetLaneModel(curRndfPath, rndfPathWidth + extraWidth, curTimestamp, numLanesLeft, numLanesRight); double avoidanceExtra = sparse ? 5 : 7.5; LinePath centerLine, leftBound, rightBound; if (boundEndDistMax != null || boundStartDistMin != null) { LinearizeStayInLane(centerLaneModel, planningDistance + avoidanceExtra, null, boundEndDistMax, boundStartDistMin, null, curTimestamp, out centerLine, out leftBound, out rightBound); } else { LinearizeStayInLane(centerLaneModel, planningDistance + avoidanceExtra, curTimestamp, out centerLine, out leftBound, out rightBound); } BehaviorManager.TraceSource.TraceEvent(TraceEventType.Verbose, 0, "in stay in lane, center line count: {0}, left bound count: {1}, right bound count: {2}", centerLine.Count, leftBound.Count, rightBound.Count); // calculate time to collision of opposing obstacle if it exists LinePath targetLine = centerLine; double targetWeight = default_lane_alpha_w; if (sparse) { targetWeight = 0.00000025; } else if (oncomingVehicleExists) { double shiftDist = -(TahoeParams.T + 1); targetLine = centerLine.ShiftLateral(shiftDist); targetWeight = 0.01; } else if (opposingLaneVehicleExists) { double timeToCollision = opposingLaneVehicleDist / (Math.Abs(vs.speed) + Math.Abs(opposingLaneVehicleSpeed)); Services.Dataset.ItemAs <double>("time to collision").Add(timeToCollision, curTimestamp); if (timeToCollision < 5) { // shift to the right double shiftDist = -TahoeParams.T / 2.0; targetLine = centerLine.ShiftLateral(shiftDist); } } // set up the planning AddTargetPath(targetLine, targetWeight); if (!sparse || boundStartDistMin != null || boundEndDistMax != null) { AddLeftBound(leftBound, true); if (!oncomingVehicleExists) { AddRightBound(rightBound, false); } } double targetDist = Math.Max(centerLine.PathLength - avoidanceExtra, planningDistance); smootherBasePath = centerLine.SubPath(centerLine.StartPoint, targetDist); maxSmootherBasePathAdvancePoint = smootherBasePath.EndPoint; double extraDist = (planningDistance + avoidanceExtra) - centerLine.PathLength; extraDist = Math.Min(extraDist, 5); if (extraDist > 0) { centerLine.Add(centerLine[centerLine.Count - 1] + centerLine.EndSegment.Vector.Normalize(extraDist)); } avoidanceBasePath = centerLine; Services.UIService.PushLineList(centerLine, curTimestamp, "subpath", true); Services.UIService.PushLineList(leftBound, curTimestamp, "left bound", true); Services.UIService.PushLineList(rightBound, curTimestamp, "right bound", true); // get the overall max speed looking forward from our current point settings.maxSpeed = GetMaxSpeed(curRndfPath, curRndfPath.AdvancePoint(curRndfPath.ZeroPoint, vs.speed * TahoeParams.actuation_delay)); // get the max speed at the end point settings.maxEndingSpeed = GetMaxSpeed(curRndfPath, curRndfPath.AdvancePoint(curRndfPath.ZeroPoint, planningDistance)); useAvoidancePath = false; if (sparse) { // limit to 5 mph laneWidthAtPathEnd = 20; pathAngleCheckMax = 50; pathAngleMax = 5 * Math.PI / 180.0; settings.maxSpeed = Math.Min(settings.maxSpeed, 2.2352); maxAvoidanceBasePathAdvancePoint = avoidanceBasePath.AdvancePoint(avoidanceBasePath.EndPoint, -2); //maxSmootherBasePathAdvancePoint = smootherBasePath.AdvancePoint(smootherBasePath.EndPoint, -2); LinePath leftEdge = RoadEdge.GetLeftEdgeLine(); LinePath rightEdge = RoadEdge.GetRightEdgeLine(); if (leftEdge != null) { leftLaneBounds.Add(new Boundary(leftEdge, 0.1, 1, 100, false)); } if (rightEdge != null) { rightLaneBounds.Add(new Boundary(rightEdge, 0.1, 1, 100, false)); } PlanningResult result = new PlanningResult(); ISteeringCommandGenerator commandGenerator = SparseArcVoting.SparcVote(ref prevCurvature, goalPoint); if (commandGenerator == null) { // we have a block, report dynamically infeasible result = OnDynamicallyInfeasible(null, null); } else { result.steeringCommandGenerator = commandGenerator; result.commandLabel = commandLabel; } Track(result, commandLabel); return; } else if (oncomingVehicleExists) { laneWidthAtPathEnd = 10; } BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "max speed set to {0}", settings.maxSpeed); disablePathAngleCheck = false; SmoothAndTrack(commandLabel, true); }
public override void Process(object param) { if (!base.BeginProcess()) { return; } if (param is ZoneTravelingBehavior) { HandleBaseBehavior((ZoneTravelingBehavior)param); } extraObstacles = GetPerimeterObstacles(); if (reverseGear) { ProcessReverse(); return; } AbsoluteTransformer absTransform = Services.StateProvider.GetAbsoluteTransformer(curTimestamp); // get the vehicle relative path LinePath relRecommendedPath = recommendedPath.Transform(absTransform); LinePath.PointOnPath zeroPoint = relRecommendedPath.ZeroPoint; // get the distance to the end point double distToEnd = relRecommendedPath.DistanceBetween(zeroPoint, relRecommendedPath.EndPoint); // get the planning distance double planningDist = GetPlanningDistance(); planningDist = Math.Max(planningDist, 20); planningDist -= zeroPoint.Location.Length; if (planningDist < 2.5) { planningDist = 2.5; } if (distToEnd < planningDist) { // make the speed command at stop speed command behaviorTimestamp = curTimestamp; speedCommand = new StopAtDistSpeedCommand(distToEnd - TahoeParams.FL); planningDist = distToEnd; approachSpeed = recommendedSpeed.Speed; settings.endingHeading = relRecommendedPath.EndSegment.UnitVector.ArcTan; settings.endingPositionFixed = true; settings.endingPositionMax = 2; settings.endingPositionMin = -2; } else { speedCommand = new ScalarSpeedCommand(recommendedSpeed.Speed); } // get the distance of the path segment we care about LinePath pathSegment = relRecommendedPath.SubPath(zeroPoint, planningDist); double avoidanceDist = planningDist + 5; avoidanceBasePath = relRecommendedPath.SubPath(zeroPoint, ref avoidanceDist); if (avoidanceDist > 0) { avoidanceBasePath.Add(avoidanceBasePath.EndPoint.Location + avoidanceBasePath.EndSegment.Vector.Normalize(avoidanceDist)); } // test if we should clear out of arc mode if (arcMode) { if (TestNormalModeClear(relRecommendedPath, zeroPoint)) { prevCurvature = double.NaN; arcMode = false; } } if (Math.Abs(zeroPoint.AlongtrackDistance(Coordinates.Zero)) > 1) { pathSegment.Insert(0, Coordinates.Zero); } else { if (pathSegment[0].DistanceTo(pathSegment[1]) < 1) { pathSegment.RemoveAt(0); } pathSegment[0] = Coordinates.Zero; } if (arcMode) { Coordinates relativeGoalPoint = relRecommendedPath.EndPoint.Location; ArcVoteZone(relativeGoalPoint, extraObstacles); return; } double pathLength = pathSegment.PathLength; if (pathLength < 6) { double additionalDist = 6.25 - pathLength; pathSegment.Add(pathSegment.EndPoint.Location + pathSegment.EndSegment.Vector.Normalize(additionalDist)); } // determine if polygons are to the left or right of the path for (int i = 0; i < zoneBadRegions.Length; i++) { Polygon poly = zoneBadRegions[i].Transform(absTransform); int numLeft = 0; int numRight = 0; foreach (LineSegment ls in pathSegment.GetSegmentEnumerator()) { for (int j = 0; j < poly.Count; j++) { if (ls.IsToLeft(poly[j])) { numLeft++; } else { numRight++; } } } if (numLeft > numRight) { // we'll consider this polygon on the left of the path //additionalLeftBounds.Add(new Boundary(poly, 0.1, 0.1, 0)); } else { //additionalRightBounds.Add(new Boundary(poly, 0.1, 0.1, 0)); } } // TODO: add zone perimeter disablePathAngleCheck = false; laneWidthAtPathEnd = 7; settings.Options.w_diff = 3; smootherBasePath = new LinePath(); smootherBasePath.Add(Coordinates.Zero); smootherBasePath.Add(pathSegment.EndPoint.Location); AddTargetPath(pathSegment, 0.005); settings.maxSpeed = recommendedSpeed.Speed; useAvoidancePath = false; SmoothAndTrack(command_label, true); }
protected virtual double GetMaxSpeed(LinePath rndfPath, LinePath.PointOnPath startingPoint) { double maxSpeed; if (speedCommand is ScalarSpeedCommand) { maxSpeed = Math.Min(((ScalarSpeedCommand)speedCommand).Speed, 15); // calculate the swerving max speed double deltaSteeringSigma = Services.TrackingManager.DeltaSteeringStdDev*180/Math.PI; if (deltaSteeringSigma > 3) { double swerveMax = Math.Max(18 - 0.2*deltaSteeringSigma, 2.5); maxSpeed = Math.Min(maxSpeed, swerveMax); } approachSpeed = null; } else { // assume that it's a stop at dist or stop at line // either way, we do the same thing if (approachSpeed == null || (double)approachSpeed < 1) { approachSpeed = Math.Max(vs.speed, 1); BehaviorManager.TraceSource.TraceEvent(TraceEventType.Information, 0, "approach speed set to {0}", approachSpeed.Value); } maxSpeed = approachSpeed.Value; } if (rndfPath != null) { // get the angles List<Pair<int, double>> angles = rndfPath.GetIntersectionAngles(startingPoint.Index, rndfPath.Count-1); foreach (Pair<int, double> angleValue in angles) { // calculate the desired speed to take the point double dist = Math.Max(rndfPath.DistanceBetween(startingPoint, rndfPath.GetPointOnPath(angleValue.Left))-TahoeParams.FL, 0); // calculate the desired speed at the intersection double desiredSpeed = 1.5/(angleValue.Right/(Math.PI/2)); // limit the desired speed to 2.5 desiredSpeed = Math.Max(2.5, desiredSpeed); // calculate the speed we would be allowed to go right now double desiredMaxSpeed = Math.Sqrt(desiredSpeed*desiredSpeed + 2*target_decel*dist); // check if the desired max speed is lower if (desiredMaxSpeed < maxSpeed) { maxSpeed = desiredMaxSpeed; } } } return maxSpeed; }
protected virtual void DetermineBlockageDistancesAndDeceleration(IList<Obstacle> obstacles, LinePath measurePath) { LinePath.PointOnPath zeroPoint = measurePath.ZeroPoint; foreach (Obstacle obs in obstacles) { if (obs.avoidanceStatus == AvoidanceStatus.Collision && obs.collisionPoints != null && obs.collisionPoints.Count > 0) { // project each obstacle point onto the measure path and get the distance double minDist = double.MaxValue; foreach (Coordinates pt in obs.collisionPoints) { LinePath.PointOnPath closestPt = measurePath.GetClosestPoint(pt); double dist = measurePath.DistanceBetween(zeroPoint, closestPt); if (dist < minDist) { minDist = dist; } } // the distance will be short by the amount we expanded out the c-space polygon minDist += TahoeParams.T / 2.0; // decrease the distance by the length to the front bumper minDist -= TahoeParams.FL; obs.obstacleDistance = minDist; minDist -= stopped_spacing_dist; // don't have negative distance if (minDist <= 0.01) { minDist = 0.01; } double decel = vs.speed*vs.speed/(2*minDist); obs.requiredDeceleration = decel; } } }