Add support for velocity path following

[icolwell-as]

I tried setting up a PV vehicle and got the following: Path-based vehicles are still not supported in GeoScenario Server 1

I think paths are useful in some cases where we just want vehicles to follow certain speed profiles.

I noticed the docs suggest the following:

Path Vehicle:
Note This Behavior type is the standard vehicle behavior in GeoScenario 1.0. 
If using GeoScenario 2.0, we recommend using vehicles with dynamic driving with the SDV model or Trajectory Vehicles.

So does it make sense for us add PV support? or is there some better way to simply specify a speed profile using trajectories or something?

[icolwell-as]

After spending more time understanding the code, I think I will implement PV, for our use cases, it's the simplest/quickest way to get the vehicle behavior we want.

[rodrigoqueiroz]

The Path Vehicle was only implemented in the Unreal WiseSIM client. Unfortunately, that part is closed source and I never port it. TVs can be used as an alternative with timed trajectories (they interpolate the vehicle movement), but they won't be as flexible. I will mark it as a feature request.

[icolwell-as]

Thanks @rodrigoqueiroz! You can assign this to me if you want, I've got a working draft. Love this codebase! it's very quick and easy to expand on.

[mantkiew]

WiseSIM's implementation is using some spline functions from Unreal API, so that would simply have to be rewritten to use another implementation (like tinyspline that we use elsewhere). I'm happy to share that code, no problem.

Ok, here's the gist of it:

1. for an agent (pedestrian and vehicle), the trajectory points are used to construct a spline
2. the agent is placed at distance 0 along the spline and then in each step the distance is increased by speed/delta time
3. this->PlaceAgentAtDistanceAlongPath(this->DistanceAlongPath); is called to move the agent
4. PlaceAgentAtDistanceAlongPath simply samples the spline to get a point and heading at the given distance from the beginning of the spline.

Now, the real complication comes from handling the speed and computing the new distance (full code Agent.zip ):

float AAgent::GetDistanceAlongSplineAtTimeFromNow(float Seconds) {
    // Estimates the agent's position at Seconds time in the future,
    // taking into account looping, and assuming no state changes
    // from triggers.

    if (this->State.PathPoints.Num() == 0) {
        return 0.f;
    }

    if (!this->State.IsInMotion ||
        FMath::IsNearlyEqual(Seconds, 0.f, 0.0001f)) {
        return this->DistanceAlongPath;
    }

    if (!this->State.UseSpeedProfile) {
        float DistanceFromNow = this->State.Speed * Seconds;
        float TotalDistance = this->DistanceAlongPath + DistanceFromNow;

        if (TotalDistance > this->Path->GetSplineLength()) {
            TotalDistance = this->Path->GetSplineLength();
        }

        return TotalDistance;
    }

    // After the loop, these will hold the properties of the correct spline
    // section
    int32 SplineIndex = floor(
      this->Path->SplineCurves.ReparamTable.Eval(this->DistanceAlongPath));
    int next_index = SplineIndex % this->Path->GetNumberOfSplinePoints();
    float InitialSpeed = this->State.Speed;
    float FinalSpeed = this->State.SpeedProfile[next_index].speed_target;
    float InitialDistance = this->DistanceAlongPath;
    float SectionDistance =
      Path->GetDistanceAlongSplineAtSplinePoint(next_index) -
      this->DistanceAlongPath;
    float SectionDuration = SectionDistance / ((InitialSpeed + FinalSpeed) / 2);

    // Find the correct section of the spline
    while (true) {
        if (SectionDuration > Seconds) {
            break;
        }

        // Incorrect section, update index and seconds
        Seconds -= SectionDuration;
        SplineIndex++;

        if (SplineIndex >= this->State.PathPoints.Num() - 1) {
            return this->Path->GetSplineLength();
        }

        // Get the amount of time this section would take based on initial
        // speed, final speed, and distance
        InitialSpeed = this->State.SpeedProfile[SplineIndex].speed_target;
        FinalSpeed = this->State
                       .SpeedProfile[(SplineIndex + 1) %
                                     this->Path->GetNumberOfSplinePoints()]
                       .speed_target;
        InitialDistance =
          this->Path->GetDistanceAlongSplineAtSplinePoint(SplineIndex);
        SectionDistance =
          this->Path->GetDistanceAlongSplineAtSplinePoint(
            (SplineIndex + 1) % this->Path->GetNumberOfSplinePoints()) -
          InitialDistance;

        SectionDuration = SectionDistance / ((InitialSpeed + FinalSpeed) / 2);
    }

    // Get the constant acceleration, knowing initial speed, final speed, and
    // time of the entire section
    float SectionAcceleration = (FinalSpeed - InitialSpeed) / SectionDuration;

    // Get the distance the agent is predicted to travel, knowing initial speed,
    // constant acceleration, and time inside the section
    // d = (vi)(t) + (1/2)(a)(t^2)
    float PredictedDistance = (InitialSpeed * Seconds) +
                              (0.5 * SectionAcceleration * Seconds * Seconds);

    float TotalDistance = InitialDistance + PredictedDistance;

    return TotalDistance;
}

There are two broad cases: constant speed or using a speed profile. In the latter case, each node has the target speed and the speed is interpolated linearly in-between the nodes. However, we also can provide the intended acceleration and time to achieve such acceleration allowing for more complex speed profiles like required by the aaa_traffic_jam_scenario.

[icolwell-as]

Thanks @mantkiew ! For now, I've got a relatively simple interpolation approach that works well and closely matches the existing trajectory approach. I'll open a PR at some point once it's ready.

Splines are a good option too, I guess it comes down to 2 approaches for users defining their scenarios:

• Define less points and use splines to get smooth movement.
• Define the smooth movement in the scenario using more points (higher resolution) for paths/trajectories.

Splines would be a good optional tag for the gs paths.