Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(vehicle_cmd_gate): variable filter limits for different ego speed #4599

Merged
merged 15 commits into from
Aug 23, 2023
Merged

feat(vehicle_cmd_gate): variable filter limits for different ego speed #4599

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
7bc3b56
feat(vehicle_cmd_gate): variable filter limits for different vehicle …
TakaHoribe Aug 11, 2023
7577aff
add steer diff test
TakaHoribe Aug 11, 2023
1bcdd8c
add node test for vehicle_cmd_gate
TakaHoribe Aug 11, 2023
b5114b6
update
TakaHoribe Aug 11, 2023
7fa9016
update
TakaHoribe Aug 12, 2023
d6ecf59
update readme
TakaHoribe Aug 16, 2023
f643f25
add is_filter_activated msg
TakaHoribe Aug 17, 2023
474e4ec
update
TakaHoribe Aug 17, 2023
3b461a0
update is_activate for each field
TakaHoribe Aug 17, 2023
aaff9c9
update
TakaHoribe Aug 17, 2023
4a64fa0
Update control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp
TakaHoribe Aug 20, 2023
e7dd395
add function test for interpolation
TakaHoribe Aug 20, 2023
693abad
fix format
TakaHoribe Aug 20, 2023
eccab7c
remove unused file
TakaHoribe Aug 21, 2023
d7ae89b
add missing depend
TakaHoribe Aug 22, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 19 additions & 1 deletion control/vehicle_cmd_gate/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,13 +13,31 @@ ament_auto_add_library(vehicle_cmd_gate_node SHARED

rclcpp_components_register_node(vehicle_cmd_gate_node
PLUGIN "vehicle_cmd_gate::VehicleCmdGate"
EXECUTABLE vehicle_cmd_gate
EXECUTABLE vehicle_cmd_gate_exe
)

rosidl_generate_interfaces(
${PROJECT_NAME}
"msg/IsFilterActivated.msg"
DEPENDENCIES builtin_interfaces
)

# to use same package defined message
if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0)
rosidl_target_interfaces(vehicle_cmd_gate_node
${PROJECT_NAME} "rosidl_typesupport_cpp")
else()
rosidl_get_typesupport_target(
cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
target_link_libraries(vehicle_cmd_gate_node "${cpp_typesupport_target}")
endif()


if(BUILD_TESTING)
ament_add_ros_isolated_gtest(test_vehicle_cmd_gate
test/src/test_main.cpp
test/src/test_vehicle_cmd_filter.cpp
test/src/test_filter_in_vehicle_cmd_gate_node.cpp
)
ament_target_dependencies(test_vehicle_cmd_gate
rclcpp
Expand Down
52 changes: 32 additions & 20 deletions control/vehicle_cmd_gate/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,26 +43,38 @@

## Parameters

| Parameter | Type | Description |
| ------------------------------------------- | ------ | --------------------------------------------------------------------------- |
| `update_period` | double | update period |
| `use_emergency_handling` | bool | true when emergency handler is used |
| `check_external_emergency_heartbeat` | bool | true when checking heartbeat for emergency stop |
| `system_emergency_heartbeat_timeout` | double | timeout for system emergency |
| `external_emergency_stop_heartbeat_timeout` | double | timeout for external emergency |
| `stop_hold_acceleration` | double | longitudinal acceleration cmd when vehicle should stop |
| `emergency_acceleration` | double | longitudinal acceleration cmd when vehicle stop with emergency |
| `moderate_stop_service_acceleration` | double | longitudinal acceleration cmd when vehicle stop with moderate stop service |
| `nominal.vel_lim` | double | limit of longitudinal velocity (activated in AUTONOMOUS operation mode) |
| `nominal.lon_acc_lim` | double | limit of longitudinal acceleration (activated in AUTONOMOUS operation mode) |
| `nominal.lon_jerk_lim` | double | limit of longitudinal jerk (activated in AUTONOMOUS operation mode) |
| `nominal.lat_acc_lim` | double | limit of lateral acceleration (activated in AUTONOMOUS operation mode) |
| `nominal.lat_jerk_lim` | double | limit of lateral jerk (activated in AUTONOMOUS operation mode) |
| `on_transition.vel_lim` | double | limit of longitudinal velocity (activated in TRANSITION operation mode) |
| `on_transition.lon_acc_lim` | double | limit of longitudinal acceleration (activated in TRANSITION operation mode) |
| `on_transition.lon_jerk_lim` | double | limit of longitudinal jerk (activated in TRANSITION operation mode) |
| `on_transition.lat_acc_lim` | double | limit of lateral acceleration (activated in TRANSITION operation mode) |
| `on_transition.lat_jerk_lim` | double | limit of lateral jerk (activated in TRANSITION operation mode) |
| Parameter | Type | Description |
| ------------------------------------------- | -------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| `update_period` | double | update period |
| `use_emergency_handling` | bool | true when emergency handler is used |
| `check_external_emergency_heartbeat` | bool | true when checking heartbeat for emergency stop |
| `system_emergency_heartbeat_timeout` | double | timeout for system emergency |
| `external_emergency_stop_heartbeat_timeout` | double | timeout for external emergency |
| `stop_hold_acceleration` | double | longitudinal acceleration cmd when vehicle should stop |
| `emergency_acceleration` | double | longitudinal acceleration cmd when vehicle stop with emergency |
| `moderate_stop_service_acceleration` | double | longitudinal acceleration cmd when vehicle stop with moderate stop service |
| `nominal.vel_lim` | double | limit of longitudinal velocity (activated in AUTONOMOUS operation mode) |
| `nominal.reference_speed_point` | <double> | velocity point used as a reference when calculate control command limit (activated in AUTONOMOUS operation mode). The size of this array must be equivalent to the size of the limit array. |
| `nominal.lon_acc_lim` | <double> | array of limits of longitudinal acceleration (activated in AUTONOMOUS operation mode) |
| `nominal.lon_jerk_lim` | <double> | array of limits of longitudinal jerk (activated in AUTONOMOUS operation mode) |
| `nominal.lat_acc_lim` | <double> | array of limits of lateral acceleration (activated in AUTONOMOUS operation mode) |
| `nominal.lat_jerk_lim` | <double> | array of limits of lateral jerk (activated in AUTONOMOUS operation mode) |
| `on_transition.vel_lim` | double | limit of longitudinal velocity (activated in TRANSITION operation mode) |
| `on_transition.reference_speed_point` | <double> | velocity point used as a reference when calculate control command limit (activated in TRANSITION operation mode). The size of this array must be equivalent to the size of the limit array. |
| `on_transition.lon_acc_lim` | <double> | array of limits of longitudinal acceleration (activated in TRANSITION operation mode) |
| `on_transition.lon_jerk_lim` | <double> | array of limits of longitudinal jerk (activated in TRANSITION operation mode) |
| `on_transition.lat_acc_lim` | <double> | array of limits of lateral acceleration (activated in TRANSITION operation mode) |
| `on_transition.lat_jerk_lim` | <double> | array of limits of lateral jerk (activated in TRANSITION operation mode) |

## Filter function

This module incorporates a limitation filter to the control command right before its published. Primarily for safety, this filter restricts the output range of all control commands published through Autoware.

The limitation values are calculated based on the 1D interpolation of the limitation array parameters. Here is an example for the longitudinal jerk limit.

![filter-example](./image/filter.png)

Notation: this filter is not designed to enhance ride comfort. Its main purpose is to detect and remove abnormal values in the control outputs during the final stages of Autoware. If this filter is frequently active, it implies the control module may need tuning. If you're aiming to smoothen the signal via a low-pass filter or similar techniques, that should be handled in the control module. When the filter is activated, the topic `~/is_filter_activated` is published.

## Assumptions / Known limits

Expand Down
22 changes: 12 additions & 10 deletions control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,15 +13,17 @@
stop_check_duration: 1.0
nominal:
vel_lim: 25.0
lon_acc_lim: 5.0
lon_jerk_lim: 5.0
lat_acc_lim: 5.0
lat_jerk_lim: 7.0
actual_steer_diff_lim: 1.0
reference_speed_points: [20.0, 30.0]
lon_acc_lim: [5.0, 4.0]
lon_jerk_lim: [5.0, 4.0]
lat_acc_lim: [5.0, 4.0]
lat_jerk_lim: [7.0, 6.0]
actual_steer_diff_lim: [1.0, 0.8]
on_transition:
vel_lim: 50.0
lon_acc_lim: 1.0
lon_jerk_lim: 0.5
lat_acc_lim: 2.0
lat_jerk_lim: 7.0
actual_steer_diff_lim: 1.0
reference_speed_points: [20.0, 30.0]
lon_acc_lim: [1.0, 0.9]
lon_jerk_lim: [0.5, 0.4]
lat_acc_lim: [2.0, 1.8]
lat_jerk_lim: [7.0, 6.0]
actual_steer_diff_lim: [1.0, 0.8]
Binary file added control/vehicle_cmd_gate/image/filter.png
View file
Open in desktop
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
2 changes: 1 addition & 1 deletion control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
<!-- vehicle info -->
<arg name="vehicle_info_param_file" default="$(find-pkg-share vehicle_info_util)/config/vehicle_info.param.yaml"/>

<node pkg="vehicle_cmd_gate" exec="vehicle_cmd_gate" name="vehicle_cmd_gate" output="screen">
<node pkg="vehicle_cmd_gate" exec="vehicle_cmd_gate_exe" name="vehicle_cmd_gate" output="screen">
<remap from="input/steering" to="/vehicle/status/steering_status"/>

<remap from="input/auto/control_cmd" to="trajectory_follower/control_cmd"/>
Expand Down
10 changes: 10 additions & 0 deletions control/vehicle_cmd_gate/msg/IsFilterActivated.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
builtin_interfaces/Time stamp

bool is_activated

# for additional information
bool is_activated_on_steering
bool is_activated_on_steering_rate
bool is_activated_on_speed
bool is_activated_on_acceleration
bool is_activated_on_jerk
6 changes: 6 additions & 0 deletions control/vehicle_cmd_gate/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,8 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<build_depend>rosidl_default_generators</build_depend>

<depend>autoware_adapi_v1_msgs</depend>
<depend>autoware_auto_control_msgs</depend>
<depend>autoware_auto_system_msgs</depend>
Expand All @@ -33,10 +35,14 @@
<depend>tier4_vehicle_msgs</depend>
<depend>vehicle_info_util</depend>
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

In my environment,
<build_depend>rosidl_default_generators</build_depend>
is needed for ros2 topic echo /control/vehicle_cmd_gate/is_filter_activated

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Addressed


<exec_depend>rosidl_default_runtime</exec_depend>

<test_depend>ament_cmake_ros</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>

<member_of_group>rosidl_interface_packages</member_of_group>

<export>
<build_type>ament_cmake</build_type>
</export>
Expand Down
Loading