-
Notifications
You must be signed in to change notification settings - Fork 639
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Signed-off-by: Takamasa Horibe <[email protected]>
- Loading branch information
1 parent
2fdb3ec
commit d04f9f1
Showing
5 changed files
with
5 additions
and
84 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
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,80 +1 @@ | ||
# Planning Error Monitor | ||
|
||
## Purpose | ||
|
||
`planning_error_monitor` checks a trajectory that if it has any invalid numerical values in its positions, twist and accel values. In addition, it also checks the distance between any two consecutive points and curvature value at a certain point. This package basically monitors if a trajectory, which is generated by planning module, has any unexpected errors. | ||
|
||
## Inner-workings / Algorithms | ||
|
||
![flow_chart_image](./media/flowchart.png) | ||
|
||
### Point Value Checker (onTrajectoryPointValueChecker) | ||
|
||
This function checks position, twist and accel values of all points on a trajectory. If they have `Nan` or `Infinity`, this function outputs error status. | ||
|
||
### Interval Checker (onTrajectoryIntervalChecker) | ||
|
||
This function computes interval distance between two consecutive points, and will output error messages if the distance is over the `interval_threshold`. | ||
|
||
### Curvature Checker (onTrajectoryCurvatureChecker) | ||
|
||
This function checks if the curvature at each point on a trajectory has an appropriate value. Calculation details are described in the following picture. First, we choose one point(green point in the picture) that are 1.0[m] behind the current point. Then we get a point(blue point in the picture) 1.0[m] ahead of the current point. Using these three points, we calculate the curvature by [this method](https://en.wikipedia.org/wiki/Menger_curvature). | ||
|
||
### Relative Angle Checker (onTrajectoryRelativeAngleChecker) | ||
|
||
This function checks if the relative angle at point1 generated from point2 and 3 on a trajectory has an appropriate value. | ||
|
||
![curvature_calculation_diagram](./media/curvature_calculation_diagram.svg) | ||
|
||
## Inputs / Outputs | ||
|
||
### Input | ||
|
||
| Name | Type | Description | | ||
| -------------------- | ---------------------------------------- | -------------------------------------- | | ||
| `~/input/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Planned Trajectory by planning modules | | ||
|
||
### Output | ||
|
||
| Name | Type | Description | | ||
| ---------------- | --------------------------------- | --------------------- | | ||
| `/diagnostics` | `diagnostic_msgs/DiagnosticArray` | diagnostics outputs | | ||
| `~/debug/marker` | `visualization_msgs/MarkerArray` | visualization markers | | ||
|
||
## Parameters | ||
|
||
| Name | Type | Description | Default value | | ||
| :------------------------ | :------- | :------------------------------------ | :------------ | | ||
| `error_interval` | `double` | Error Interval Distance Threshold [m] | 100.0 | | ||
| `error_curvature` | `double` | Error Curvature Threshold | 1.0 | | ||
| `error_sharp_angle` | `double` | Error Sharp Angle Threshold | $\pi$/4 | | ||
| `ignore_too_close_points` | `double` | Ignore Too Close Distance Threshold | 0.005 | | ||
|
||
## Visualization | ||
|
||
When the trajectory error occurs, markers for visualization are published at the topic `~/debug/marker`. | ||
|
||
- trajectory_interval: An error occurs when the distance between two points exceeds a certain large value. The two points where the error occurred will be visualized. | ||
- trajectory_curvature: An error occurs when the curvature exceeds a certain large value. The three points used to calculate the curvature will be visualized. | ||
- trajectory_relative_angle: An error occurs when the angle in the direction of the path point changes significantly. The three points used to calculate the relative angle will be visualized. | ||
|
||
## Assumptions / Known limits | ||
|
||
- It cannot compute curvature values at start and end points of the trajectory. | ||
- If trajectory points are too close, curvature calculation might output incorrect values. | ||
|
||
## Future extensions / Unimplemented parts | ||
|
||
- Collision checker with obstacles may be implemented in the future. | ||
|
||
## Error detection and handling | ||
|
||
For the onsite validation, you can use the `invalid_trajectory_publisher` node. Please launch the node with the following command when the target trajectory is being published. | ||
|
||
```bash | ||
ros2 launch planning_error_monitor invalid_trajectory_publisher.launch.xml | ||
``` | ||
|
||
This node subscribes the target trajectory, inserts the invalid point, and publishes it with the same name. The invalid trajectory is supposed to be detected by the `planning_error_monitor`. | ||
|
||
Limitation: Once the `invalid_trajectory_publisher` receives the trajectory, it will turn off the subscriber. This is to prevent the trajectory from looping in the same node, therefore, only the one pattern of invalid trajectory is generated. | ||
# Planning Validator |
2 changes: 1 addition & 1 deletion
2
planning/planning_validator/include/planning_validator/debug_marker.hpp
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
2 changes: 1 addition & 1 deletion
2
planning/planning_validator/include/planning_validator/planning_validator.hpp
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