Merge pull request #1192 from tier4/doc/lane_pose_calculation

Doc/lane pose calculation

.github/pull_request_samples/example_detail.md

@@ -28,8 +28,8 @@ This PR fixes how the length of the curve is computed
 
 ## References
 
-- [determine-arc-length-of-a-catmull-rom-spline-to-move-at-a-constant-speed](https://gamedev.stackexchange.com/questions/14985/determine-arc-length-of-a-catmull-rom-spline-to-move-at-a-constant-speed)
-  - This link is an example and is not directly related to this sample.
+See also [this document.](https://tier4.github.io/scenario_simulator_v2-docs/)  
+This link is an example and is not directly related to this sample.
 
 # Destructive Changes
 

docs/developer_guide/lane_pose_calculation/GetLongitudinalDistance.md

@@ -0,0 +1,8 @@
+# Lane pose calculation when getting longitudinal distance
+
+Attempts to calculate the pose for adjacent lane coordinate systems when measuring longitudinal distance.  
+The length of the horizontal bar must intersect with the adjacent lanelet, [so it is always 10 m regardless of the entity type.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/entity/entity_manager.cpp#L375C21-L442)
+
+![Get longitudinal distance](../../image/longitudinal_distance.png "Getting longitudinal distance.")
+
+The shorter of the two blue arrows shown in the figure above is the longitudinal distance between entities.

docs/developer_guide/lane_pose_calculation/LanePoseCalculation.md

@@ -0,0 +1,83 @@
+# Lane pose calculation
+
+The calculation method of the lane coordinate system differs from Entity to Entity, and the process of determining which lane to match is complex, including fallback.  
+In this document, we will show how the pose calculation of the lane coordinate system is performed for each Entity.
+
+There are 3 possible timings when the lane coordinate system is computed for all Entities.  
+These are the [timing immediately after the spawn of the Entity](Spawn.md), the [timing of the frame update](UpdateFrame.md) and the [timing of the getting longitudinal distance](GetLongitudinalDistance.md)
+
+!!! Note
+    It is sufficient to check [this document](UpdateFrame.md) and [this section](#lane-coordinate-system-calculation-algorithm-for-a-specific-lane), if you are not a simulator developer but a scenario writer. That is because the distance measurement via condition of the OpenSCENARIO is done after frame update basically.
+
+## Lane coordinate system calculation algorithm for a specific lane
+
+!!! summary
+    - The calculation of the pose in the lane coordinate system involves two steps: first, filtering by "which lanes can be matched," and then "calculating the pose in the specific lane coordinate system.  
+    - The parameter that most affects the matching results is the length of the horizontal bar. This length depends on the type of entity and the timing of the calculation. If you want to check the length quickly, please check [this section](#quick-guide-to-horizontal-bar-lengths).
+
+### Quick guide to horizontal bar lengths 
+
+The unit of the table is meter.
+
+| EntityType \ Timing of calculation | [Spawn](Spawn.md) | [UpdateFrame](UpdateFrame.md)                                                          | [Getting longitudinal distance](GetLongitudinalDistance.md) |
+| ---------------------------------- | ----------------- | -------------------------------------------------------------------------------------- | ----------------------------------------------------------- |
+| EgoEntity                          | 2.0               | [(tread of the entity) + 2.0](UpdateFrame.md#calculate-pose-in-lane-coordinate-system) | 10                                                          |
+| VehicleEntity                      | 2.0               | N/A                                                                                    | 10                                                          |
+| PedestrianEntity                   | 2.0               | [2.0 or 4.0](UpdateFrame.md#pedestrian-entity-with-behavior-tree)                      | 10                                                          |
+| MiscObject                         | 2.0               | N/A                                                                                    | 10                                                          |
+
+### Detail of the lane coordinate system calculation algorithm for a specific lane
+
+After the lanes to be matched have been determined, the calculation of the specific lane coordinate system to be performed is always based on a per spline curve and line segment determination.
+
+![Lane pose calculation](../../image/lane_pose_calculation.png "Lane pose calculation.")
+
+The centerline of a lane is expressed by the cubic equation [Catmull-Rom spline.](https://en.wikipedia.org/wiki/Centripetal_Catmull%E2%80%93Rom_spline)  
+Catmull-Rom spline and line segment are implemented by finding the multiple solution of one cubic equation.  
+A specific implementation can be found [here](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/common/math/geometry/src/solver/polynomial_solver.cpp#L98-L131) and [here.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/common/math/geometry/src/spline/hermite_curve.cpp#L124-L187)
+
+Let $a_i,b_i,c_i,d_i (i = x, y, z)$ $s = [0,1]$ is a coefficient of the spline.  
+Then, the spline is a described like below.  
+
+$$x(s) = a_xs^3 + b_xs^2 + c_xs + d_x $$
+
+$$y(s) = a_ys^3 + b_ys^2 + c_ys + d_y $$
+
+$$z(s) = a_zs^3 + b_ys^2 + c_zs + d_z $$
+
+When a hit decision with a line segment is performed, only $x(s),y(s)$ of the above equations are considered, since they are performed in two dimensions.  
+
+The equation of the line segment is expressed as.
+Let $e_i,f_i (i = x, y, z)$ $u = [0,1]$ is a coefficient of the line segment.  
+
+$$x(u) = e_xu + f_x $$
+
+$$y(u) = e_yu + f_y $$
+
+$$z(u) = e_zu + f_z $$
+
+When a hit decision with a spline is performed, only $x(u),y(u)$ of the above equations are considered, since they are performed in two dimensions.  
+
+To determine the pertinence of a line segment and a spline, find the values of s and u that satisfy the following equations.  
+
+$$x(s) = x(u) = a_xs^3 + b_xs^2 + c_xs + d_x = e_xu + f_x$$
+
+$$y(s) = y(u) = a_ys^3 + b_ys^2 + c_ys + d_y = e_yu + f_y$$
+
+If $e_x\neq0$
+Perform the equation transformation and eliminate the variable $u$.
+
+$$x(u)\frac{e_f}{e_x} = (e_xu + f_x)\frac{e_f}{e_x} = e_fu +\frac{f_xe_f}{e_x} = x(s)\frac{e_f}{e_x} = (a_xs^3 + b_xs^2 + c_xs + d_x)\frac{e_f}{e_x}$$
+
+$$y(s) - x(u)\frac{e_f}{e_x} = a_ys^3 + b_ys^2 + c_ys + d_y - (a_xs^3 + b_xs^2 + c_xs + d_x)\frac{e_f}{e_x} = e_yu + f_y - (e_xu + f_x)\frac{e_f}{e_x} = f_y - \frac{f_xe_f}{e_x}$$
+
+Therefore, we can perform a hit decision between the Spline curve and the line segment by finding a rational solution of the following cubic equation.
+
+$$(a_y - a_x\frac{e_f}{e_x})s^3 + (b_y - b_x\frac{e_f}{e_x})s^2 + (c_y - c_x\frac{e_f}{e_x})s + d_y - d_x\frac{e_f}{e_x} - f_y + \frac{f_xe_f}{e_x} = 0$$
+
+After getting rational solution $s$, calculate $u$ from the linear function.
+If the $s$ and $u$ is in $[0,1]$, Catmull-Rom spline and the line segment are determined to be in conflict.
+
+The value of $S$ obtained in this case is the coordinate $S$ in the lane coordinate system.  
+Next, the value of $s$ is substituted into the Catmull-Rom spline equation, and the tangent direction at that point is calculated.  
+The Euler angle is calculated from the difference in orientation between the obtained tangent direction and Entity.

docs/developer_guide/lane_pose_calculation/Spawn.md

@@ -0,0 +1,33 @@
+# Lane pose calculation when spawning
+
+If you spawn entities by specifying in the lane coordinate system, lane pose calculation is skipped.  
+Internally, spawning entities by specifying in the lane coordinate system means specifying LaneletPose as the argument pose of the [API::spawn function](https://tier4.github.io/scenario_simulator_v2-api-docs/classtraffic__simulator_1_1API.html).
+In the OpenSCENARIO, it is to execute a TeleportAction by specifying the LanePosition. ([e.g](https://github.com/tier4/scenario_simulator_v2/blob/9f03394f80e5de05cf087db3d00c7be73d27e963/test_runner/scenario_test_runner/))
+
+If you spawn entities by specifying in the map coordinate system, [lane pose calculation](https://github.com/tier4/scenario_simulator_v2/blob/9f03394f80e5de05cf087db3d00c7be73d27e963/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp#L521-L529) is performed.
+
+There are several internal implementations of pose calculation in the lane coordinate system implemented in the [HDmapUtils class](https://tier4.github.io/scenario_simulator_v2-api-docs/classhdmap__utils_1_1HdMapUtils.html).
+The one used in this case is [here](https://github.com/tier4/scenario_simulator_v2/blob/9f03394f80e5de05cf087db3d00c7be73d27e963/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp#L602-L630).
+
+In this function, the following procedure is used to calculate the pose in the lane coordinate system.
+
+## Search for matching lanes
+
+This procedure falls back to 2 if 1 fails.
+If 2 also fails, fallback is performed to 3.
+If 3 also fails, fallback is performed to 4.
+
+1. [Obtain candidate lanes for matching considering the bounding box.](https://github.com/tier4/scenario_simulator_v2/blob/9f03394f80e5de05cf087db3d00c7be73d27e963/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp#L607)
+1. [Obtain lanes in the neighborhood of the Entity as matching candidates without considering the bounding box.](https://github.com/tier4/scenario_simulator_v2/blob/9f03394f80e5de05cf087db3d00c7be73d27e963/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp#L608-L610)
+1. [Obtain matching candidates before and after the lane matched by considering the bounding box.](https://github.com/tier4/scenario_simulator_v2/blob/9f03394f80e5de05cf087db3d00c7be73d27e963/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp#L611-L628)
+1. [Obtain matching candidates that exist in close proximity (0.1m) to the Entity.](https://github.com/tier4/scenario_simulator_v2/blob/9f03394f80e5de05cf087db3d00c7be73d27e963/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp#L629C10-L629C23)
+
+If all cases from 1~4 fail, the pose in the lane coordinate system will be a calculation failure.
+
+## Calculate pose in lane coordinate system
+
+After narrowing down the candidate lanes by the above procedure, the pose of the lane coordinate system is calculated from the coordinates of the intersection of the horizontal bar extended from the Entity and the Catmull-Rom Spline curve constructed from the center line of the lane for each candidate lane.
+
+![Lane pose calculation](../../image/lane_pose_calculation.png "Lane pose calculation.")
+
+In this case, the length of the horizontal bar is `2.0`m.

docs/developer_guide/lane_pose_calculation/UpdateFrame.md

@@ -0,0 +1,69 @@
+# Lane pose calculation when updating frames
+
+If the entity's behavioral logic is planned in the lane coordinate system, skip this process.
+
+## Ego Entity
+
+Since EgoEntity is controlled in map coordinates by Autoware rather than in lane coordinates using a motion plugin, this process is performed at each frame update.  
+This process is implemented [here](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/api/api.cpp#L240C9-L240C19) and [here](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/entity/ego_entity.cpp#L276-L312).
+
+### Search for matching lanes
+This process branches off into 2 or 3 depending on the outcome of 1.  
+If 3 is executed and fails, fallbacks to 4.
+
+1. [Obtain a candidate lane for matching from Autoware planner output.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/entity/ego_entity.cpp#L276)
+2. [If Autoware has no planner output, try obtaining candidate lanes for matching considering the bounding box.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/entity/ego_entity.cpp#L286-L287)
+3. [If Autoware has planner output, try matching considering the Autoware planner output.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/entity/ego_entity.cpp#L289-L290)
+4. [If the step 3 failed, try obtaining candidate lanes for matching considering the bounding box.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/entity/ego_entity.cpp#L292-L293)
+
+### Calculate pose in lane coordinate system
+
+Let $L_m$ be the length of the horizontal bar used in the lane coordinate system calculation and the tread of the front wheels be  $t_f$ and the tread of the rear wheels be $t_r$.
+See also [here.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/entity/ego_entity.cpp#L278-L284)
+
+$$L_m = max(t_r, t_f) + 2.0$$
+
+![Lane pose calculation](../../image/lane_pose_calculation.png "Lane pose calculation.")
+
+## Non-Ego Entity
+
+For non-EgoEntity, the process diverges depending on whether planning is done in the lane coordinate system inside BehaviorPlugin.  
+If planning is done in the lane coordinate system, there is no need to convert from the map coordinate system to the lane coordinate system after update frame function is executed.
+
+### VehicleEntity (with Behavior-Tree)
+
+Only during `follow_polyline_trajectory` execution is planning performed in map coordinate system, but [lane coordinate system calculations are not performed](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp#L546)
+
+### Vehicle Entity (with Do-Nothing)
+
+While the do-nothing behavior plugin is running, Entity does not move, so the lane coordinate system is calculated when spawning entity and is not recalculated thereafter.
+
+### Pedestrian Entity (with Behavior-Tree)
+
+Planning is done in map coordinates in the `walk_straight` and `follow_polyline_trajectory` actions. 
+
+In the `walk_straight` Action, [the pose in the lane coordinate system is calculated.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L56)  
+The procedure for calculating the pose in the lane coordinate system at this time is as follows.  
+
+If lane matching was successful in the previous frame, do 1, otherwise do 2
+
+1.[Set the length of the horizontal bar to 2.0 and calculate the pose in the lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c 27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L72-L77)  
+2.[Calculate the pose in the lane coordinate system considering the size of the BoundingBox of the Entity](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0 c27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L79-L83)  
+
+If calculation 1 or 2 fails,  
+3.[Set the length of the horizontal bar to 4.0 and calculate the pose in the lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c 27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L86-L91)  
+If 1 or 2 are successful, then 3 is skipped.
+
+If the pose could not be calculated in the lane coordinate system by considering up to the result of 3, [the pose calculation in the lane coordinate system is a failure](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L125).
+
+[Canonicalize pose in lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L94) to determine the final pose in the lane coordinate system.
+
+In the `follow_polyline_trajectory` Action, [lane coordinate system calculations are not performed](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp#L546)
+
+### Pedestrian Entity (with Do-Nothing)
+
+While the do-nothing behavior plugin is running, Entity does not move, so the lane coordinate system is calculated when spawning entity and is not recalculated thereafter.
+
+### MiscObjectEntity
+
+Misc object entity does not move, so the lane coordinate system is calculated when spawning entity and is not recalculated thereafter.

mkdocs.yml

@@ -80,15 +80,20 @@ nav:
       - developer_guide/AutowareAPI.md
       - developer_guide/NPCBehavior.md
       - developer_guide/BehaviorPlugin.md
-      #      - developer_guide/ErrorCategories.md
+      # - developer_guide/ErrorCategories.md
       - developer_guide/OpenSCENARIOSupport.md
       - developer_guide/SimpleSensorSimulator.md
       - developer_guide/SystemArchitecture.md
       - developer_guide/TrafficSimulator.md
       - developer_guide/ZeroMQ.md
       - developer_guide/VehicleDynamics.md
       - developer_guide/SimulationResultFormat.md
-# cspell: ignore TIERIV
+      - Lane Pose Calculation:
+          Timing of lane pose calculation: developer_guide/lane_pose_calculation/LanePoseCalculation.md
+          Process of lane pose calculation when spawning entity: developer_guide/lane_pose_calculation/Spawn.md
+          Process of lane pose calculation when updating frame: developer_guide/lane_pose_calculation/UpdateFrame.md
+          Process of lane pose calculation when getting longitudinal distance: developer_guide/lane_pose_calculation/GetLongitudinalDistance.md
+      # cspell: ignore TIERIV
       - developer_guide/TIERIVScenarioFormatVersion2.md
       - developer_guide/CONTRIBUTING.md
       - developer_guide/Communication.md