-
Notifications
You must be signed in to change notification settings - Fork 60
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2…
… into feature/split_rviz_packages
- Loading branch information
Showing
60 changed files
with
909 additions
and
30 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -2,7 +2,7 @@ | |
| <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> | ||
| <package format="3"> | ||
| <name>arithmetic</name> | ||
| <version>1.0.1</version> | ||
| <version>1.0.2</version> | ||
| <description>arithmetic library for scenario_simulator_v2</description> | ||
| <maintainer email="[email protected]">Tatsuya Yamasaki</maintainer> | ||
| <license>Apache License 2.0</license> | ||
|
|
||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -2,7 +2,7 @@ | |
| <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> | ||
| <package format="3"> | ||
| <name>geometry</name> | ||
| <version>1.0.1</version> | ||
| <version>1.0.2</version> | ||
| <description>geometry math library for scenario_simulator_v2 application</description> | ||
| <maintainer email="[email protected]">Masaya Kataoka</maintainer> | ||
| <license>Apache License 2.0</license> | ||
|
|
||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -2,7 +2,7 @@ | |
| <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> | ||
| <package format="3"> | ||
| <name>scenario_simulator_exception</name> | ||
| <version>1.0.1</version> | ||
| <version>1.0.2</version> | ||
| <description>Exception types for scenario simulator</description> | ||
| <maintainer email="[email protected]">Tatsuya Yamasaki</maintainer> | ||
| <license>Apache License 2.0</license> | ||
|
|
||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -2,7 +2,7 @@ | |
| <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> | ||
| <package format="3"> | ||
| <name>simple_junit</name> | ||
| <version>1.0.1</version> | ||
| <version>1.0.2</version> | ||
| <description>Lightweight JUnit library for ROS 2</description> | ||
| <maintainer email="[email protected]">Masaya Kataoka</maintainer> | ||
| <maintainer email="[email protected]">Tatsuya Yamasaki</maintainer> | ||
|
|
||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -2,7 +2,7 @@ | |
| <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> | ||
| <package format="3"> | ||
| <name>status_monitor</name> | ||
| <version>1.0.1</version> | ||
| <version>1.0.2</version> | ||
| <description>none</description> | ||
| <maintainer email="[email protected]">Tatsuya Yamasaki</maintainer> | ||
| <license>Apache License 2.0</license> | ||
|
|
||
8 changes: 8 additions & 0 deletions
8
docs/developer_guide/lane_pose_calculation/GetLongitudinalDistance.md
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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) | ||
|
|
||
|  | ||
|
|
||
| The shorter of the two blue arrows shown in the figure above is the longitudinal distance between entities. |
83 changes: 83 additions & 0 deletions
83
docs/developer_guide/lane_pose_calculation/LanePoseCalculation.md
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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. | ||
|
|
||
|  | ||
|
|
||
| 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. |
Oops, something went wrong.