Skip to content

Commit

Permalink
feat(operation_transition_mannager): add param enable_engage_on_drivi…
Browse files Browse the repository at this point in the history 
…ng (autowarefoundation#4891)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
  • Loading branch information
@TakaHoribe
TakaHoribe authored Sep 7, 2023
1 parent 0ab6a3b commit 114f72a
Show file tree
Hide file tree
Showing 4 changed files with 40 additions and 8 deletions.
30 changes: 23 additions & 7 deletions control/operation_mode_transition_manager/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,13 +63,14 @@ For the backward compatibility (to be removed):

## Parameters

| Name | Type | Description | Default value |
| :--------------------------------- | :------- | :------------------------------------------------------------------------------------------------ | :------------ |
| `transition_timeout` | `double` | If the state transition is not completed within this time, it is considered a transition failure. | 10.0 |
| `frequency_hz` | `double` | running hz | 10.0 |
| `check_engage_condition` | `double` | If false, autonomous transition is always available | 0.1 |
| `nearest_dist_deviation_threshold` | `double` | distance threshold used to find nearest trajectory point | 3.0 |
| `nearest_yaw_deviation_threshold` | `double` | angle threshold used to find nearest trajectory point | 1.57 |
| Name | Type | Description | Default value |
| :--------------------------------- | :------- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
| `transition_timeout` | `double` | If the state transition is not completed within this time, it is considered a transition failure. | 10.0 |
| `frequency_hz` | `double` | running hz | 10.0 |
| `enable_engage_on_driving` | `bool` | Set true if you want to engage the autonomous driving mode while the vehicle is driving. If set to false, it will deny Engage in any situation where the vehicle speed is not zero. Note that if you use this feature without adjusting the parameters, it may cause issues like sudden deceleration. Before using, please ensure the engage condition and the vehicle_cmd_gate transition filter are appropriately adjusted. | 0.1 |
| `check_engage_condition` | `bool` | If false, autonomous transition is always available | 0.1 |
| `nearest_dist_deviation_threshold` | `double` | distance threshold used to find nearest trajectory point | 3.0 |
| `nearest_yaw_deviation_threshold` | `double` | angle threshold used to find nearest trajectory point | 1.57 |

For `engage_acceptable_limits` related parameters:

Expand All @@ -94,6 +95,21 @@ For `stable_check` related parameters:
| `speed_upper_threshold` | `double` | the velocity deviation between control command and ego vehicle must be within this threshold to complete `Autonomous` transition. | 2.0 |
| `speed_lower_threshold` | `double` | the velocity deviation between control command and ego vehicle must be within this threshold to complete `Autonomous` transition. | 2.0 |

## Engage check behavior on each parameter setting

This matrix describes the scenarios in which the vehicle can be engaged based on the combinations of parameter settings:

| `enable_engage_on_driving` | `check_engage_condition` | `allow_autonomous_in_stopped` | Scenarios where engage is permitted |
| :------------------------: | :----------------------: | :---------------------------: | :---------------------------------------------------------------- |
| x | x | x | Only when the vehicle is stationary. |
| x | x | o | Only when the vehicle is stationary. |
| x | o | x | When the vehicle is stationary and all engage conditions are met. |
| x | o | o | Only when the vehicle is stationary. |
| o | x | x | At any time (Caution: Not recommended). |
| o | x | o | At any time (Caution: Not recommended). |
| o | o | x | When all engage conditions are met, regardless of vehicle status. |
| o | o | o | When all engage conditions are met or the vehicle is stationary. |

## Future extensions / Unimplemented parts

- Need to remove backward compatibility interfaces.
Expand Down
7 changes: 6 additions & 1 deletion ...rol/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,11 +2,16 @@
ros__parameters:
transition_timeout: 10.0
frequency_hz: 10.0

# set true if you want to engage the autonomous driving mode while the vehicle is driving. If set to false, it will deny Engage in any situation where the vehicle speed is not zero. Note that if you use this feature without adjusting the parameters, it may cause issues like sudden deceleration. Before using, please ensure the engage condition and the vehicle_cmd_gate transition filter are appropriately adjusted.
enable_engage_on_driving: false

check_engage_condition: false # set false if you do not want to care about the engage condition.

nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
engage_acceptable_limits:
allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed.
allow_autonomous_in_stopped: true # if true, all engage check is skipped.
dist_threshold: 1.5
yaw_threshold: 0.524
speed_upper_threshold: 10.0
Expand Down
10 changes: 10 additions & 0 deletions control/operation_mode_transition_manager/src/state.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,7 @@ AutonomousMode::AutonomousMode(rclcpp::Node * node)
"trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = *msg; });

check_engage_condition_ = node->declare_parameter<bool>("check_engage_condition");
enable_engage_on_driving_ = node->declare_parameter<bool>("enable_engage_on_driving");
nearest_dist_deviation_threshold_ =
node->declare_parameter<double>("nearest_dist_deviation_threshold");
nearest_yaw_deviation_threshold_ =
Expand Down Expand Up @@ -218,6 +219,15 @@ bool AutonomousMode::isModeChangeAvailable()
const auto target_control_speed = control_cmd_.longitudinal.speed;
const auto & param = engage_acceptable_param_;

if (!enable_engage_on_driving_ && std::fabs(current_speed) > 1.0e-2) {
RCLCPP_INFO(
logger_,
"Engage unavailable: enable_engage_on_driving is false, and the vehicle is not "
"stationary.");
debug_info_ = DebugInfo{}; // all false
return false;
}

if (trajectory_.points.size() < 2) {
RCLCPP_WARN_SKIPFIRST_THROTTLE(
logger_, *clock_, 5000, "Engage unavailable: trajectory size must be > 2");
Expand Down
1 change: 1 addition & 0 deletions control/operation_mode_transition_manager/src/state.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,6 +73,7 @@ class AutonomousMode : public ModeChangeBase
rclcpp::Clock::SharedPtr clock_;

bool check_engage_condition_ = true; // if false, the vehicle is engaged without any checks.
bool enable_engage_on_driving_ = false; // if false, engage is not permited on driving
double nearest_dist_deviation_threshold_; // [m] for finding nearest index
double nearest_yaw_deviation_threshold_; // [rad] for finding nearest index
EngageAcceptableParam engage_acceptable_param_;
Expand Down

0 comments on commit 114f72a

Please sign in to comment.