public BadPointOnPath(Coordinates xy, IRelativePath path, CarTimestamp timestamp, object data)
{
this.xy = xy;
this.path = path;
this.data = data;
this.timestamp = timestamp;
}
public BadPointOnPath(Coordinates xy, IRelativePath path, CarTimestamp timestamp, object data)
{
this.xy = xy;
this.path = path;
this.data = data;
this.timestamp = timestamp;
}
public static IAbsolutePath Combine(this AbsoluteDirectoryPath basePath, IRelativePath path)
{
return(path == null
? basePath
: path.MatchWith(
(RelativeFilePath file) => (IAbsolutePath)basePath.Combine(file),
(RelativeDirectoryPath dir) => (IAbsolutePath)basePath.Combine(dir)));
}
public PathSteeringCommandGenerator(IRelativePath path)
{
if (path == null) {
throw new ArgumentNullException("path");
}
this.path = path;
}
public PathSteeringCommandGenerator(IRelativePath path)
{
if (path == null)
{
throw new ArgumentNullException("path");
}
this.path = path;
}
private IRelativePath Combine(IRelativePath entry, string?pathParamName, string?formatParamName)
{
var mutualFormat = PathFormat.GetMutualFormat(PathFormat, entry.PathFormat);
if (mutualFormat == null)
{
throw new ArgumentException("Cannot combine path formats that are not universal or do not match.", formatParamName);
}
if (PathDisplay.Length == 0)
{
return(entry);
}
if (entry.PathDisplay.Length == 0)
{
return(this);
}
var appendPath = entry.PathFormat.SplitRelativeNavigation(entry.PathDisplay, out int parentDirs);
appendPath = PathFormat.ConvertRelativePathToMutualFormat(appendPath, entry.PathFormat, mutualFormat);
StringOrSpan basePath = GetBasePathForAppending(parentDirs) ??
throw new ArgumentException("Invalid path combination: Attempt to navigate past root directory.", pathParamName);
basePath = PathFormat.ConvertRelativePathToMutualFormat(basePath, PathFormat, mutualFormat);
string newPath = appendPath.Length > 0 || parentDirs == -1 ?
StringHelper.Concat(basePath, PathFormat.SeparatorString, appendPath) : (string)basePath;
if (entry.IsDirectory)
{
return(new Impl(newPath, RootLength, PathFormat));
}
else
{
return(new IRelativeFilePath.Impl(newPath, RootLength, PathFormat));
}
}
public static TrackingCommand GetStopAtStoplinePathCommand(IRelativePath path, double baseSpeed)
{
return(new TrackingCommand(new FeedbackSpeedCommandGenerator(new StopSpeedGenerator(new StoplineDistanceProvider(), baseSpeed)),
new PathSteeringCommandGenerator(path), false));
}
public static void ComputeSteeringCommand(IRelativePath path, double dt, bool outputDataset, out double steeringWheelAngle)
{
// get steering Parameters
double q1 = q1_param.Value;
double q2 = q2_param.Value;
double r = r_param.Value;
OperationalVehicleState vs = Services.StateProvider.GetVehicleState();
// get the feedback data
SteeringControlData data = path.GetSteeringControlData(new SteeringControlDataOptions(vs.speed*TahoeParams.actuation_delay));
double offtrackError = data.offtrackError;
double headingError = data.headingError;
// for now, just use 0 for the curvature if it is null
// later, we may want to switch to pure-pursuit in this case
double curvature = data.curvature.GetValueOrDefault(0);
// cap the off-track error term
if (offtrackError > offtrack_error_max_param.Value) {
offtrackError = Math.Sign(offtrackError)*offtrack_error_max_param.Value;
}
// adjust the heading error 180 degrees if we're in reverse
if (vs.transGear == TransmissionGear.Reverse) {
headingError += Math.PI;
}
// wrap heading error between -pi and pi
headingError = Math.IEEERemainder(headingError, 2*Math.PI);
// check for numerical problems
if (double.IsNaN(headingError)) {
headingError = 0;
Console.WriteLine("Error: Heading Error is NaN (In OpPathFollowingBehavior:Process)");
}
else if (Math.Abs(headingError) > Math.PI) {
headingError -= Math.Sign(headingError)*2*Math.PI;
}
// compute desired curvature
double desiredCurvature = (offtrackError * Math.Sqrt(q1 / r)) +
(headingError * Math.Sqrt(q2 / r)) +
(curvature );
// reverse desired curvature if in reverse gear
if (vs.transGear == TransmissionGear.Reverse) {
desiredCurvature = -desiredCurvature;
}
// check desired curvature
if (double.IsNaN(desiredCurvature)) {
Console.WriteLine("Error: Commanded Curvature is NaN (In OpPathFollowingBehavior:Process)");
desiredCurvature = 0;
}
// add the commands to the dataset
if (outputDataset) {
CarTimestamp now = Services.CarTime.Now;
Services.Dataset.ItemAs<double>("offtrack error").Add(offtrackError, now);
Services.Dataset.ItemAs<double>("heading error").Add(headingError, now);
Services.Dataset.ItemAs<double>("target curvature").Add(curvature, now);
Services.Dataset.ItemAs<double>("commanded curvature").Add(desiredCurvature, now);
//Operational.Instance.Dataset.ItemAs<Coordinates>("path tangent").Add(curPoint.segment.Tangent(curPoint).Normalize(), now);
}
// convert the desired curvature to a steering wheel angle and return it
steeringWheelAngle = SteeringUtilities.CurvatureToSteeringWheelAngle(desiredCurvature, vs.speed);
}
public static void ComputeSteeringCommand(IRelativePath path, double dt, out double steeringWheelAngle)
{
ComputeSteeringCommand(path, dt, true, out steeringWheelAngle);
}
public static TrackingCommand GetStopAtStoplinePathCommand(IRelativePath path, double baseSpeed)
{
return new TrackingCommand(new FeedbackSpeedCommandGenerator(new StopSpeedGenerator(new StoplineDistanceProvider(), baseSpeed)),
new PathSteeringCommandGenerator(path), false);
}
public static IRelativePath Rename(this IRelativePath path, string newName)
{
return(path.MatchWith(
(RelativeFilePath file) => (IRelativePath)file.Rename(new FileName(newName)),
(RelativeDirectoryPath dir) => (IRelativePath)dir.Rename(new DirectoryName(newName))));
}
public static void ComputeSteeringCommand(IRelativePath path, double dt, bool outputDataset, out double steeringWheelAngle)
{
// get steering Parameters
double q1 = q1_param.Value;
double q2 = q2_param.Value;
double r = r_param.Value;
OperationalVehicleState vs = Services.StateProvider.GetVehicleState();
// get the feedback data
SteeringControlData data = path.GetSteeringControlData(new SteeringControlDataOptions(vs.speed * TahoeParams.actuation_delay));
double offtrackError = data.offtrackError;
double headingError = data.headingError;
// for now, just use 0 for the curvature if it is null
// later, we may want to switch to pure-pursuit in this case
double curvature = data.curvature.GetValueOrDefault(0);
// cap the off-track error term
if (offtrackError > offtrack_error_max_param.Value)
{
offtrackError = Math.Sign(offtrackError) * offtrack_error_max_param.Value;
}
// adjust the heading error 180 degrees if we're in reverse
if (vs.transGear == TransmissionGear.Reverse)
{
headingError += Math.PI;
}
// wrap heading error between -pi and pi
headingError = Math.IEEERemainder(headingError, 2 * Math.PI);
// check for numerical problems
if (double.IsNaN(headingError))
{
headingError = 0;
Console.WriteLine("Error: Heading Error is NaN (In OpPathFollowingBehavior:Process)");
}
else if (Math.Abs(headingError) > Math.PI)
{
headingError -= Math.Sign(headingError) * 2 * Math.PI;
}
// compute desired curvature
double desiredCurvature = (offtrackError * Math.Sqrt(q1 / r)) +
(headingError * Math.Sqrt(q2 / r)) +
(curvature);
// reverse desired curvature if in reverse gear
if (vs.transGear == TransmissionGear.Reverse)
{
desiredCurvature = -desiredCurvature;
}
// check desired curvature
if (double.IsNaN(desiredCurvature))
{
Console.WriteLine("Error: Commanded Curvature is NaN (In OpPathFollowingBehavior:Process)");
desiredCurvature = 0;
}
// add the commands to the dataset
if (outputDataset)
{
CarTimestamp now = Services.CarTime.Now;
Services.Dataset.ItemAs <double>("offtrack error").Add(offtrackError, now);
Services.Dataset.ItemAs <double>("heading error").Add(headingError, now);
Services.Dataset.ItemAs <double>("target curvature").Add(curvature, now);
Services.Dataset.ItemAs <double>("commanded curvature").Add(desiredCurvature, now);
//Operational.Instance.Dataset.ItemAs<Coordinates>("path tangent").Add(curPoint.segment.Tangent(curPoint).Normalize(), now);
}
// convert the desired curvature to a steering wheel angle and return it
steeringWheelAngle = SteeringUtilities.CurvatureToSteeringWheelAngle(desiredCurvature, vs.speed);
}
public static void ComputeSteeringCommand(IRelativePath path, double dt, out double steeringWheelAngle)
{
ComputeSteeringCommand(path, dt, true, out steeringWheelAngle);
}
public IRelativePath Combine(IRelativePath entry) => Combine(entry, nameof(entry), nameof(entry));
