Skip to content

Commit 8a9b86b

Browse files
author
Ahmed Ebrahim
committed
feat(remaining_dist_eta): add check for velocity limit preventing unintended division by zero or negative time values / update readme
Signed-off-by: Ahmed Ebrahim <ahmed.ebrahim@leodrive.ai> feat(remaining_dist_eta): add check for velocity limit preventing unintended division by zero or negative time values / update readme Signed-off-by: Ahmed Ebrahim <ahmed.ebrahim@leodrive.ai>
1 parent 650eb80 commit 8a9b86b

File tree

2 files changed

+14
-11
lines changed

2 files changed

+14
-11
lines changed

planning/autoware_remaining_distance_time_calculator/README.md

+11-10
Original file line numberDiff line numberDiff line change
@@ -2,12 +2,12 @@
22

33
### Role
44

5-
This package aims to provide mission remaining distance and remaining time calculations
5+
This package aims to provide mission remaining distance and remaining time calculations.
66

77
### Activation and Timing
88

9-
- The calculations are activated once we have a route planned for a mission in Autoware
10-
- The calculations are triggered timely based on the `update_rate` parameter
9+
- The calculations are activated once we have a route planned for a mission in Autoware.
10+
- The calculations are triggered timely based on the `update_rate` parameter.
1111

1212
### Module Parameters
1313

@@ -20,19 +20,20 @@ This package aims to provide mission remaining distance and remaining time calcu
2020
#### Remaining Distance Calculation
2121

2222
- The remaining distance calculation is based on getting the remaining shortest path between the current vehicle pose and goal pose using `lanelet2` routing APIs.
23-
- The remaining distance is calculated by summing the 2D length of remaining shortest path, with exception to current lanelet and goal lanelet
24-
- For the current lanelet, the distance is calculated from the current vehicle position to the end of that lanelet
23+
- The remaining distance is calculated by summing the 2D length of remaining shortest path, with exception to current lanelet and goal lanelet.
24+
- For the current lanelet, the distance is calculated from the current vehicle position to the end of that lanelet.
2525
- For the goal lanelet, the distance is calculated from the start of the lanelet to the goal pose in this lanelet.
2626
- When there is only one lanelet remaining, the distance is calculated by getting the 2D distance between the current vehicle pose and goal pose.
27-
- Checks are added to handle cases when current lanelent, goal lanelet, or routing graph are not valid to prevent node process die
28-
- In such cases when, last valid remaining distance and time are maintained
27+
- Checks are added to handle cases when current lanelent, goal lanelet, or routing graph are not valid to prevent node process die.
28+
- In such cases when, last valid remaining distance and time are maintained.
2929

3030
#### Remaining Time Calculation
3131

3232
- The remaining time currently depends on a simple equation of motion by getting the maximum velocity limit.
33-
- The remaining distance is calculated by dividing remaining distance by the maximum velocity limit
33+
- The remaining distance is calculated by dividing the remaining distance by the maximum velocity limit.
34+
- A check is added to the remaining time calculation to make sure that maximum velocity limit is greater than zero. This prevents division by zero or getting negative time value.
3435

3536
### Future Work
3637

37-
- Find a more efficient way for remaining distance calculation instead of regularly searching the graph for finding the remaining shortest path
38-
- Engage more sophisticated motion models for more accurate remaining time calculations
38+
- Find a more efficient way for remaining distance calculation instead of regularly searching the graph for finding the remaining shortest path.
39+
- Engage more sophisticated motion models for more accurate remaining time calculations.

planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp

+3-1
Original file line numberDiff line numberDiff line change
@@ -167,7 +167,9 @@ void RemainingDistanceTimeCalculatorNode::calculate_remaining_distance()
167167

168168
void RemainingDistanceTimeCalculatorNode::calculate_remaining_time()
169169
{
170-
remaining_time_ = remaining_distance_ / velocity_limit_;
170+
if (velocity_limit_ > 0.0) {
171+
remaining_time_ = remaining_distance_ / velocity_limit_;
172+
}
171173
}
172174

173175
void RemainingDistanceTimeCalculatorNode::publish_mission_remaining_distance_time()

0 commit comments

Comments
 (0)