lci_strategic_plugin hangs when trying to find entry on green signal #2228
Conversation
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
9 tasks
MishkaMN
changed the title
adamlm
previously approved these changes
Dec 19, 2023
adamlm
suggested changes
Dec 20, 2023
| * NOTE: TSMO UC2: Algorithm 2. Entering time estimation algorithm for EVs in cooperation Class A when the SPaT plan is fixed-time (defined for C-ADS-equipped vehicles) | ||
| */ | ||
|
|
||
| rclcpp::Time get_nearest_green_entry_time(const rclcpp::Time& current_time, const rclcpp::Time& earliest_entry_time, lanelet::CarmaTrafficSignalPtr signal, double minimum_required_green_time = 0.0) const; | ||
| std::tuple<rclcpp::Time, bool> get_nearest_valid_entry_time(const rclcpp::Time& current_time, const rclcpp::Time& earliest_entry_time, lanelet::CarmaTrafficSignalPtr signal, double minimum_required_green_time = 0.0) const; |
There was a problem hiding this comment.
Consider changing this to std::optional<rclcpp::Time> to indicate that a nearest valid entry time does not exist under the given function arguments.
| @@ -49,7 +49,21 @@ double LCIStrategicPlugin::get_distance_to_accel_or_decel_once (double current_s | |||
| } | |||
| } | |||
|
|
|||
| rclcpp::Time LCIStrategicPlugin::get_nearest_green_entry_time(const rclcpp::Time& current_time, const rclcpp::Time& earliest_entry_time, lanelet::CarmaTrafficSignalPtr signal, double minimum_required_green_time) const | |||
| rclcpp::Time get_eet_or_tbd(const rclcpp::Time& earliest_entry_time, const lanelet::CarmaTrafficSignalPtr& signal) | |||
There was a problem hiding this comment.
See comment about std::optional.
| nearest_green_entry_time = get_nearest_green_entry_time(current_state.stamp, earliest_entry_time, traffic_light) | ||
| + rclcpp::Duration(EPSILON * 1e9); //0.01sec more buffer since green_light algorithm's timestamp picks the previous signal - Vehicle Estimation | ||
| is_entry_time_within_green_or_tbd = true; | ||
| std::tie(nearest_green_entry_time, in_tbd) = get_nearest_valid_entry_time(current_state.stamp, earliest_entry_time, traffic_light); |
There was a problem hiding this comment.
See comment about std::optional.
adamlm
approved these changes
Dec 21, 2023
|
This was tested locally to be able to build. The autoware.ai's carma-develop branch is pushing to wrong hub.docker.com image tag (carma-develop instead of develop) which is why the CI is failing. Therefore force merging this and force pushing to |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
PR Details
Description
Currently the lci_strategic_plugin is not accounting for an edge case where if there is a green signal in the received signal states, but that signal is behind the vehicle's earliest entry time (EET) possible due to the vehicle's kinematics.
It is only handling the case if there is no green signal at all. Therefore it may erroneously continue to look for the green signal forever.
Applied fix is to return the start of TBD state which is the end of the available states with RED signal. Since function can return a timestamp and state that is not GREEN, renamed the function.
Supporting PR to be merged first: usdot-fhwa-stol/autoware.ai#261
Related GitHub Issue
#2227
Related Jira Key
CDAR-619
https://usdot-carma.atlassian.net/browse/CDAR-619
Motivation and Context
How Has This Been Tested?
Simulation PC 1
Types of changes
Checklist: