[UCDP开发文档] 一、轨迹模块

本文用于介绍UCDP软件中轨迹模块的功能、数据类型和应用接口。
This documentation is to introduce the trajectory module in UCDP software, including functionality, data type and API.

1. Module Directory

| - planner/
| - - TrajectoryController.py
| - - TrajectoryWindow.py
| - - csv/
| - - pic/
| - - scenarios/
| - - - NCAP/
| - - - Userdefined/
| - - toolbox/
| - - - toolbox_FileList.py
| - - - toolbox_IniRefPosition.py
| - - - toolbox_Path.py
| - - - toolbox_PathOffset.py
| - - - toolbox_SceneView.py
| - - - toolbox_SetWidth.py
| - - - toolbox_TimeLapse.py
| - - - toolbox_TrajectoryTree.py
| - - - toolbox_Trigger.py
| - - trajectory/
| - - - trajectory.py
| - - - chechdescription.py
| - - - descriptionplugin.py
| - - - trajectoryhandler.py
| - - - plugin/
| - - - - trajectpry.plugin
| - - - trglib/
| - - - - trigger.py
| - - - - TrgDistance.py
| - - - - TrgGate.py
| - - - - TrgNone.py
| - - - trjlib/
| - - - - description.py
| - - - - basic/
| - - - - - TrjArc.py
| - - - - - TrjClothoid.py
| - - - - - TrjLine.py
| - - - - - TrjPause.py
| - - - - - TrjSimpleCutin.py
| - - - - NCAP/
| - - - - - C2C/
| - - - - - - TrjNCAP_CCRb.py
| - - - - - - TrjNCAP_CCRm.py
| - - - - - - TrjNCAP_CCFtap.py
| - - - - - - TrjNCAP_CCFtapVUT.py
| - - - - - CP/
| - - - - - - TrjNCAP_CPNA.py

2. Functionality Overview

UCDP的轨迹模块主要负责生成供自主移动或无人驾驶平台可执行的轨迹文件。如界面所示，点击界面工具栏中的按钮可对应地在QTreeWidget中生成对应的轨迹类型(如直线、圆弧、单移线)。这些轨迹全部需要采用参数进行定义，然后软件后台自动进行运算和拼接组合。同时，所有轨迹还可以保存到本地。
This module is particularly created for trajectory generator. As shown in the GUI, an action in the toolbar is triggered to create a related description(Line, Arc, Cutin, etc), which is displayed in the QTreeWidget and defined via certain parameters. A complete and executable trajectory composed of descriptions is calculated automatically and can be save as local files.

其他窗口的主要功能为：Scenario View 中同步显示QTreeWidget 中定义好的轨迹；TimeLapse显示轨迹的时域信息；Current Edit Trajectory显示当前编辑的轨迹的文字信息。
For other widgets, trajectory determined by descriptions is shown in Scenario View in real-time; and trajectory in time domain is figured in TimeLapse including velocity, acceleration and displacement; Current Edit Trajectory gives some text information about the trajectory.

文本中Description和Trajectory(轨迹)的区别与联系如下图所示：
What is description or trajectory? Following picture may help you understand this.

Description表示一条完整轨迹的一部分，拥有确定的起点速度和终点速度。此外，在该段轨迹上的任意一点的时刻、加速度和航向角都是唯一确定的。一条Trajectory至少包含一项Description.
A description is used to describe a part of trajectory, which must have a definite start speed and end speed. In addition, acceleration and heading angle at any point or any time in this part of trajectory is clear. In this way, we call this part of trajectory a description.

然而，并非所有的Description都可以拼接成一条有效的轨迹。我们认为，一条可供自主移动平台执行的轨迹必须从静止开始启动，最终要停止下来。同时，在整个轨迹中，速度和轨迹的航向要保持连续，不能发生突变。
However, not all descriptions can make up a correct and executable trajectory for a self-driving platform. In our opinion, a trajectory must have a zero start speed and must stop statically at the end point. In another word, a trajectory must meet the following requirements:

1. Zero start speed and end speed;
2. Speed and heading angle must be continuous throughout the travel time.

因此，仅当所有的Description首尾速度和航向角光滑衔接时才有可能组成一条完整的轨迹。
In this way, descriptions can compose a trajectory only when thier start speed and end speed have a smooth connection as well as their heading angle.

3. API Documentation

3.1 AbstractDescription

AbstractDescription是所有Description的基类，提供了Description的定义、修改、校核和状态获取的接口。

class AbstractDescription(metaclass=ABCMeta):
    def __init__(self, id: int= None, keywords: List[str] = list(), validFlag: bool= False):
        self.duration: float = 0.0
        self._id: int = id
        self._state: str = self.inistate()
        self._keywords: List[str] = keywords
        self._validFlag: bool = validFlag

在UCDP中，一条轨迹可以由属性各异的Description构成，这些属性也称为描述参数。通过这些参数，可以唯一确定一条完整的轨迹。特别注意的是，任何继承于AbstractDescription的Description类的描述参数分为必选参数和可选参数。其中，必选参数在Description类构造函数中作为成员属性定义，而可选参数通过__slots__属性进行限定。
例如：对于一条匀变速直线运动的LineDescription，必选参数为初速度(speedStart)，可选参数有末速度(endSpeed)、加速度(acceleration)和位移(distance)。

AbstractDescription is the abstract base class of all descriptions, providing defination, modification, verification and state-feedback of a description. In UCDP, a trajectory can be made up of various descriptions with different properties, also named after description paramaters. With these parameters, coordinate, speed, acceleration and heading angle at any point and any time in the trajectory is definite. Specifically, parameters are classified into required parameters and optional parameters, defined in __init__ and __slots__, respectively. Example shows the required and optimal parameters in a LineDescription.

class LineDescription(AbstractDescription):
    __slots__ = [
    'speedEnd',
    'distance',
    'acceleration',
    ]
    def __init__(self, speedStart: float = 0.0):
        super(LineDescription, self).__init__()
        self.speedStart: float = speedStart

3.1.1 Property

public duration: float

---

整个Description所需耗时
Time spent from start point to end point throughout the description.

private id: int

---

Description在Trajectory中的序列编号，按时间流动方向排序，从0开始
Serial number for every description in a trajectory in order.

private state: str

---

Description所刻画的运动状态，比如匀速运动，减速运动或加速运动
Movement state of a description, e.g., constant speed movement, uniform variable speed movement.

private keywords: list

---

描述Description的实际选用的可选参数的集合
Optional parameters used to help defined the description.

private validFlag: bool

---

Description有效标志位
Wthether the description is valid or not.

3.1.2 Method

public abstract str typename()

---

返回Description的类型名称
This method returns the name of description

public abstract int optionalnumbers()

---

一般而言，并不需要Description的全部可选参数来对其进行辅助定义，只需要其中一部分参数即可。该方法说明Description可选参数的具体个数。
例如：对于匀变速直线运动的Description，如果我们选择初速度作为必选参数，可选参数有末速度、加速度、位移，那么只需要在可选参数中任意选择两个参数就可以完整定义匀变速直线运动。
Typically, there are several optional parameters in __slots__, but not all are required. This method indicates how many parameters are needed.
e.g., Start Speed is a required parameter for a Line Description, and __slots__=[End Speed, Displacement, Acceleration] contains all optional parameters. For a uniformly variable linear motion, if we already know the Start Speed, we only need to choose 2 parameters in __slots__ as auxiliary to help define a Line Description.

public abstract void checkrequired(**kwargs)
param kwargs: 字典类型，所有的键与必选参数同名，所有的值对应用户输入的必选参数的值。

---

当用户输入完必选参数的值后，该方法用来对用户输入的参数进行校核。
例如：一个Description拥有一个"SpeedStart"的必选参数，用户输入好该值后，就可以采用以下代码对用户输入的值进行判断是否为正值，可以在改方法中使用异常等操作。
When user try to assign values to the required parameters, the method is uesd to check if these values are correct. kwargs is a dict and contains all the required parameter names(key) and their values(value).
e.g., if a description has a required parameter named “SpeedStart”, You can use assert to check wthether the parameter is positive or you can throw an Exception.

assert kwargs["SpeedStart"] > 0.0, \
 "Start speed must be positive"

public abstract void checkoptional(**kwargs)

---

该方法使用与 checkrequired(**kwargs)相似。
See public void checkrequired(**kwargs)

public abstract float calculatespeedstart()

---

返回Description的起始速度。
Returns the start speed of description.

public abstract float calculatespeedend()

---

返回Description的终止速度。如果终止速度是Description的一个必选参数/可选参数，那就直接返回该值，反之则需要通过其他参数计算得到。
Returns the end speed of description. If the end speed is one of required/optional parameters, you can return it here. However, if you cannot get the end speed directly, you’ve to calculate this with other known parameters.
e.g., you can use start speed, acceleration and displacement to calculate end speed in a uniformly variable linear motion.

public abstract float calculateacceleration()

---

返回一个加速度。该方法主要针对匀变速运动。用法与calculatespeedend()类似。
This method is particularly for uniformly variable linear motion description to get an acceleration in description. If acceleration is one of required parameter, just return it here. Otherwise, calculate it with other known parameters.

public abstract float calculatecurvelength()

---

返回Description的总路程。用法与calculatespeedend()类似。
Returns the total length of description. If curve length is one of required parameter, just return it here. Otherwise, calculate it with other known parameters.

public abstract float calculateduration()

---

返回Description的需要的总时间。注意，重写这个方法的时候你可能需要对一些其他参数进行判断校核，可以在方法中使用异常。
例如：对于一个匀变速直线运动，有两种方法可以获得时间，1). 已知初速度、末速度和加速度求时间；2). 已知初速度、末速度和位移求时间。
Returns the total time spent in description. To get this value, you may need to check some parameter values and definations. assert and Exception can be used here.
e.g., for a uniformly variable linear motion, you get two ways to obtain duration, 1). start speed, end speed and acceleration; 2). start speed, end speed and displacement;

def calculateduration(self) -> float:
   assert abs(self.speedStart) + abs(self.calculatespeedend()) != 0.0, \
       "SpeedStart and SpeedEnd can not set to zero simultaneously"
   if self.calculateacceleration() < 1e-10:
       duration = 2 * self.calculatecurvelength() / (self.speedStart + self.calculatespeedend())
   else:
       duration = (self.calculatespeedend() - self.speedStart) / self.calculateacceleration()
   self.duration = duration
   assert duration > 0.0, \
       "Travel time must be positive"
   return duration

public abstract float calculateangle()

---

该方法主要与圆弧类Description相关，计算一段圆弧对应的圆心角。对于直线，其返回值为0；对于非圆弧类的其他曲线，应该返回一个与Description相关的角度。
This method is particularly for a circular motion to get the central angle of an arc-related description. For line description, return 0.

public abstract str calculatestate()

---

返回Description的运动状态，默认实现一个匀变速运动的状态——加速、减速和匀速。
The default method is complemented based on a uniform variable speed movement.

  @abstractmethod
  def calculatestate(self) -> str:
      '''
      TODO: to calculate the state of description
      # the default method is complemented by a uniform variable speed movement
      :return: Acceleration, Deceleration, Constant, Static
      '''
      state = self.inistate()
      acceleration = self.calculateacceleration()
      if abs(acceleration) < 1e-10:
          state = "Constant"
      elif acceleration > 1e-10:
          state = "Acceleration"
      elif acceleration < -1e-10:
          state = "Deceleration"
      return state

public abstract Tuple[float] headingandxy(distance: float)
return: (x, y, heading, math.cos(heading), math.sin(heading))

---

返回一条Description上给定位移distance处的点坐标和航向角。
To calculate the heading and coordinate x and y at a given distance point in description.

public abstract Dict[str, List[Union[str, float]]] requiredUItransfer()
返回与UI界面交互的Description必选参数和只读参数相关的字典；
returns a dict which connect the UI data and description required parameters;

---

以LineDescription为例，dict({SpeedStart [km/h]: 36})为必选参数dict({speedStart:10})的界面代理参数，3.6为界面代理参数值与speedStart属性值的转换比例；*Duration [s]为duration属性在界面的代理对象，*表示只读属性。
e.g., for LineDescription, dict({SpeedStart [km/h]: 36}) is a proxy parameter in UI for required parameter dict({speedStart:10}) and 3.6 is a coefficient to transer SpeedStart [km/h] value to speedStart. *Duration [s] is a proxy for duration and * means readonly property.

def requiredUItransfer(self):
    return {
        "SpeedStart [km/h]": ["speedStart", 3.6],
        "*Duration [s]": ["duration", 1]
    }

public abstract Dict[str, List[Union[str, float]]] optionalUItransfer()
返回与UI界面交互的Description可选参数相关的字典；
returns a dict which connect the UI data and description optional parameters;

---

以LineDescription为例，dict({SpeedEnd [km/h]: 36})为可选参数dict({speedEnd:10})的界面代理参数，3.6为界面代理参数值与speedEnd属性值的转换比例；self.calculatespeedend为获取speedEnd的回调函数。
e.g., for LineDescription, dict({SpeedEnd [km/h]: 36}) is a proxy parameter in UI for required parameter dict({speedEnd:10}) and 3.6 is a coefficient to transer SpeedEnd [km/h] value to speedEnd. self.calculatespeedend is a callback to obtain speedEnd value.

def optionalUItransfer(self):
    return {"SpeedEnd [km/h]": ["speedEnd", 3.6, self.calculatespeedend],
            "Distance [m]": ["distance", 1, self.calculatecurvelength],
            "Acceleration [m/s^2]": ["acceleration", 1, self.calculateacceleration]}

public abstract numpy.ndarray calculatecoordinate(id: int, timeStepSize: float)
param timeStepSize: 轨迹的采样时间步长；
param id: Description在Trajectory中的编号；
return: [x, y, velocity, heading, headingx, headingy, timeStamp, acceleration, distance, id]
返回矩阵每列的内容：[x坐标，y坐标，速度，航向角，航向角x轴分量，航向角y轴分量，时间戳，加速度，位移，序号]

---

Description中的核心方法，用来计算一条Description在给定的时间步长(timeStepSize)下，所有轨迹点的速度、加速度、航向角、位移以及对应的时间戳。
现在，我们需要定义一下simple/basic description和composite description。
如果在一个Description类中没有依赖到其他的Description，那么成这类Description为simple/basic description，反之则为composite description。
对于simple/basic description，再实现或重写该方法的时候往往给需要进行一些数学运算来确定返回值，比如对于一个匀变速运动，可能就需要用下面的公式。
然而对于composite description来说，可以采用Trajectory 类中的一些方法将若干simple/basic description或者composite description直接组合拼接，得到一个全新的、更复杂的Description。这将在Trajectory 类的介绍中详细说明。

This is a core method to calculate coordinate x and y, velocity, acceleration, heading and distance of all trajectory points at a fixed sample time(timeStepSize).
Here, we need to introduce simple/basic description and composite description.
If a description class does not depend on other description classes, it is called a simple/basic description. For these descriptions, user may need to realize some mathematic calculation for movement states in this method, e.g., for a uniform variable speed movement, following formula may need. This abstract method is implemented by default with a uniform variable speed movement.
If a description class depends on other description classes, it is called a composite description. For these descriptions, user can use Trajectory class to easily assemble other basic or composite descriptions in this method. More details can be found in Trajectory.

public abstract AbstractTrigger gettrigger()

---

返回一个AbstractTrigger类型的触发条件。仅当该Description为composite description且独立构成一条完整的标准测试轨迹时重写该方法。
This method returns a specific trigger pattern(See AbstractTrigger) and overrides only when description is a composite description and included in an executable standard test trajectory alone.

public void checkmovingforward()

---

校核Description是否为一条向前运动的轨迹。
This method checks whether self-driving in forward direction. Exceptions can be raised here.

public void checklongitudinalacceleration()

---

校核纵向加速度是否在要求的范围内。
To check whether the longitudinal acceleration is within range.

public void checklateralacceleration()

---

校核侧向加速度是否在要求的范围内。
To check whether the lateral acceleration is within range.

private void setrequired(**kwargs)
param kwargs: 含义与checkrequired(**kwargs)形参含义相同；

---

用户输入完必选参数的值后，将用户输入的值设置为必选参数的值，即更改Description属性的值。方法首先判断用户输入的值知否都能转化为浮点数，然后调用checkrequired(**kwargs)，所有参数确认无误后修改Description的必选参数属性。
Once user finishes inputing values for required parameters, firstly to check whether these values can be transfer to float type; then call checkrequired(**kwargs). If no exception raised, required parameters are assigned new values.

private void setoptional(**kwargs)
param kwargs: 含义与checkoptional(**kwargs)形参含义相同；

---

用户输入完可选参数的值后，将用户输入的值设置为可选参数的值，即更改Description的__slots__属性的值。方法首先判断用户选择的可选参数的个数是否正确；其次，判断每个属性值能为转换为浮点数；之后，调用checkreoptional(**kwargs)；然后，调用clearoptional()清除Description以前定义好的可选参数；最后对用户选择的可选参数进行赋值。
Once user choose optional parameters and finishes inputing values, firstly call optionalnumbers(), secondly check data type for float transfer, then call checkreoptional(**kwargs), finnally call clearoptional() to clear previously chosen optional parameters and assign values to new optional parameters.

public Dict[str, float] getrequired()

---

返回__dict__属性，即必选参数属性。
returns required parameters in __dict__

public Dict[str, float] getoptional()

---

返回__slots__中用户选择并定义了的参数。
returns defined optional parameters in __slots__

public void clearoptional()

---

清除当前Description已选择的所有可选参数属性。

public void setattributes(requiredKwargs: dict, optionalKwargs: dict):

---

该方法仅提供一个统一接口同时设置可选参数和必选参数，同时调用setrequired(**kwargs)和setoptional(**kwargs)

public Dict[str, float] getdescriptionkwargs():

---

获取Description的必选参数、只读参数和可选参数对应的界面代理参数的键值对，仅当validFlag为True时有效。
return the UI proxy paramters of required parameters, readonly parameters and optional parameters.

public str inistate():

---

返回Description的初始状态，用户自定义。
returns initial state of description, user defined.

public override __str__():

---

重载返回当前Description类名称、必选参数和可选参数。
override __str__ to get description class name, required parameters and optional parameters.

3.1.3 Demo Show

3.1.3.1 LineDescription
Source code please see TrjLine.py
Here is how to use:

line = LineDescription()
line.setattributes({"speedStart": 5.0}, {"speedEnd": 10.0, "acceleration": 2.0})
points = line.calculatecoordinate(0, 0.01)

1. Firstly to instantiate a LineDescription;
2. Then, to set description parameters, including required parameters and optional parameters, you can try different parameters or combinations to see what will happen;
3. Thirdly, to calculate the description coordinates, you’ve to assign description id and sample time(timeStepSize);
4. If you can run step1 to 3 successfully, you’ve defined a line description successfully and one more thing significant is to change its valid flag:

line.validFlag = True

5. Now, you can get all description parameters:

print(line.getdescriptionkwargs())

and the output is

{'SpeedStart [km/h]': 18.0, '*Duration [s]': 2.5, 'SpeedEnd [km/h]': 36.0, 'Distance [m]': 18.75, 'Acceleration [m/s^2]': 2.0}

These values can be used in UI directly cause the units have been transfered.
6. Futhermore, all description movement states are stored in var points. For example, you can get coordinates information from points[:, 0] and points[:, 1]

import matplotlib.pyplot as plt
plt.plot(points[:, 0], points[:, 1])# plot x and y coordinate
plt.show()

or you can view velocity in time domain:

import matplotlib.pyplot as plt
plt.plot(points[:, 6], points[:, 2]) # plot time and velocity
plt.show()

More information in var points please see:

public abstract numpy.ndarray calculatecoordinate(id: int, timeStepSize: float)

3.1.3.2 NCAP_CCRmDescription
Source code please see NCAP_CCRm.py
NCAP CCRm 是标准的AEB测试工况，该轨迹由四段LineDecription构成，id为0的一段为匀加速度，id为1的为匀速段，该段为正式试验段，id为2的为匀速段，该段为试验结束但在正式减速前的缓冲段，id为3的为匀减速段。
NCAP CCRm is a standard test for AEB functions. This trajectory is made up of four line descriptions, id=0: acceleration, id=1: constant speed (under test), id=2: constant speed (test over before deceleration), id=3: deceleration.

Similarly, instantiate, assign values and calculate:

ccrb = NCAP_CCRmDescription()
ccrb.setattributes({"acceleration": 2.0, "ATPSpeed": 10.0, "deceleration": -2.0, "VUTSpeed": 20.0,
             "stayLength": 10.0, "observationTime": 5.0, "lngTechValue": 2.0}, {})
points = ccrb.calculatecoordinate(0, 0.01)

ccrb.validFlag = True
print(ccrb.getdescriptionkwargs())

and output is

{'Acceleration [m/s^2]': 2.0, 'ATPSpeed [km/h]': 36.0, 'Deceleration [m/s^2]': -2.0, 'VUTSpeed [km/h]': 72.0, 'StayLength [m]': 10.0, 'ObservationTime [s]': 5.0, 'LongitudinalTeach [m]': 2.0, '*Duration [s]': 16.0}

time vs velocity:

time vs distance:

3.2 AbstractTrigger

AbstractTrigger是所有触发类的基类，提供设置、修改、获取触发条件以及和用户界面交互的接口。
AbstractTrigger is the base class for all triggers, including DistanceTrigger, GateTrigger, NoneTrigger now.

3.2.1 Class Property

DistanceTrigger: int = 0:

---

距离触发的类型编号。
Distance Trigger type id.

GateTrigger: int = 1:

---

门触发的类型编号。
Gate Trigger type id.

NoneTrigger: int = 3:

---

空触发的类型编号， 使用空触发类型可避免方法返回None类型。
None Trigger type id, to avoid returning None type.

3.2.2 Method

public abstract int type()

---

返回Trigger的类型编号, 0: DistanceTrigger, 1: GateTrigger, 3: NoneTrigger。
returns the type id of trigger.

public abstract str strtriggername()

---

获取触发的名称。
returns th name of trigger.

public abstract str strtriggername()

---

获取触发的名称。
returns th name of trigger.

public abstract Dict[str, List[Union[str, float]]] interfaceUItransfer()

---

返回关联UI界面参数和Trigger设置参数的字典。
returns a dict which connect the UI data and trigger parameters.

public abstract checktrigger(trigger: Dict[str, float])
param trigger: configuration parameters of trigger in dict;

---

校核用户设置的触发参数是否合理。
to check whether the trigger configureation parameters set by users are correct.

public abstract Dict[str, float] triggervalues(self):

---

获取tigeer当前的设置参数。
returns the current configuration parameters fo trigger.

public settrigger(trigger: Dict[str, float]):
param trigger: configuration parameters of trigger.

---

为trigger参数设置值，首先调用checktrigger(trigger: Dict[str, float])校核参数，然后尽心设置。
to set the configuration parameters for current trigger. Firstly, checktrigger(trigger: Dict[str, float]) is called then to set trigger attributes.

public override __str__:

---

重载__str__返回当前触发的名称以及触发参数。
override __str__ to return the name and configuration parameters of trigger.