forked from autowarefoundation/autoware.universe
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat: add external_velocity_limit_selector (autowarefoundation#45)
* Feature/add external velocity limit selector (autowarefoundation#2217) * add external velocity limit selector * add README.md * improve document * apply document template * update external velocity limit selector & use commot params * update smoother & use commot params * update launch file * Fix/external velocity limit selector handling inf velocity (autowarefoundation#2241) * Change formatter to clang-format and black (autowarefoundation#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake <[email protected]> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <[email protected]> * Apply Black Signed-off-by: Kenji Miyake <[email protected]> * Apply clang-format Signed-off-by: Kenji Miyake <[email protected]> * Fix build errors Signed-off-by: Kenji Miyake <[email protected]> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <[email protected]> * Apply clang-format Signed-off-by: Kenji Miyake <[email protected]> * Fix build errors Signed-off-by: Kenji Miyake <[email protected]> * Add COLCON_IGNORE (autowarefoundation#500) Signed-off-by: Kenji Miyake <[email protected]> * delete COLCON_IGNORE (autowarefoundation#518) * update iv_msgs -> auto_msgs in planning readme (autowarefoundation#576) * update iv_msgs -> auto_msgs in planning readme * minor change * some fix * some fix Co-authored-by: Takayuki Murooka <[email protected]> * fix readme * add default parameter files Co-authored-by: Satoshi OTA <[email protected]> Co-authored-by: Kenji Miyake <[email protected]> Co-authored-by: Sugatyon <[email protected]> Co-authored-by: Takayuki Murooka <[email protected]> Co-authored-by: Takayuki Murooka <[email protected]>
- Loading branch information
1 parent
ed10202
commit c5f6056
Showing
9 changed files
with
502 additions
and
0 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 |
---|---|---|
@@ -0,0 +1,43 @@ | ||
cmake_minimum_required(VERSION 3.5) | ||
project(external_velocity_limit_selector) | ||
|
||
### Compile options | ||
if(NOT CMAKE_CXX_STANDARD) | ||
set(CMAKE_CXX_STANDARD 14) | ||
set(CMAKE_CXX_STANDARD_REQUIRED ON) | ||
set(CMAKE_CXX_EXTENSIONS OFF) | ||
endif() | ||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") | ||
add_compile_options(-Wall -Wextra -Wpedantic) | ||
endif() | ||
|
||
## Dependencies | ||
find_package(ament_cmake_auto REQUIRED) | ||
ament_auto_find_build_dependencies() | ||
|
||
# Target | ||
|
||
## Target executable | ||
set(EXTERNAL_VELOCITY_LIMIT_SELECTOR_SRC | ||
src/external_velocity_limit_selector_node.cpp | ||
) | ||
|
||
## external_velocity_limit_selector_node | ||
ament_auto_add_library(external_velocity_limit_selector_node SHARED | ||
${EXTERNAL_VELOCITY_LIMIT_SELECTOR_SRC} | ||
) | ||
|
||
rclcpp_components_register_node(external_velocity_limit_selector_node | ||
PLUGIN "ExternalVelocityLimitSelectorNode" | ||
EXECUTABLE external_velocity_limit_selector | ||
) | ||
|
||
if(BUILD_TESTING) | ||
find_package(ament_lint_auto REQUIRED) | ||
ament_lint_auto_find_test_dependencies() | ||
endif() | ||
|
||
ament_auto_package(INSTALL_TO_SHARE | ||
launch | ||
config | ||
) |
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 |
---|---|---|
@@ -0,0 +1,113 @@ | ||
# External Velocity Limit Selector | ||
|
||
## Purpose | ||
|
||
The `external_velocity_limit_selector_node` is a node that keeps consistency of external velocity limits. This module subscribes | ||
|
||
1. velocity limit command sent by **API**, | ||
2. velocity limit command sent by **Autoware internal modules**. | ||
|
||
VelocityLimit.msg contains not only **max velocity** but also information about the **acceleration/jerk constraints** on deceleration. The `external_velocity_limit_selector_node` integrates the lowest velocity limit and the highest jerk constraint to calculate the **hardest velocity limit** that protects all the deceleration points and max velocities sent by API and Autoware internal modules. | ||
|
||
![selector algorithm](./image/external_velocity_limit_selector.png) | ||
|
||
## Inner-workings / Algorithms | ||
|
||
WIP | ||
|
||
<!-- Write how this package works. Flowcharts and figures are great. Add sub-sections as you like. | ||
Example: | ||
### Flowcharts | ||
...(PlantUML or something) | ||
### State Transitions | ||
...(PlantUML or something) | ||
### How to filter target obstacles | ||
... | ||
### How to optimize trajectory | ||
... | ||
--> | ||
|
||
## Inputs | ||
|
||
| Name | Type | Description | | ||
| --------------------------------------------------- | ------------------------------------------------- | --------------------------------------------- | | ||
| `~input/velocity_limit_from_api` | autoware_planning_msgs::VelocityLimit | velocity limit from api | | ||
| `~input/velocity_limit_from_internal` | autoware_planning_msgs::VelocityLimit | velocity limit from autoware internal modules | | ||
| `~input/velocity_limit_clear_command_from_internal` | autoware_planning_msgs::VelocityLimitClearCommand | velocity limit clear command | | ||
|
||
## Outputs | ||
|
||
| Name | Type | Description | | ||
| ---------------------- | ------------------------------------- | ------------------------------------------------- | | ||
| `~output/max_velocity` | autoware_planning_msgs::VelocityLimit | current information of the hardest velocity limit | | ||
|
||
## Parameters | ||
|
||
| Parameter | Type | Description | | ||
| ----------------- | ------ | ------------------------------------------ | | ||
| `max_velocity` | double | default max velocity [m/s] | | ||
| `normal.min_acc` | double | minimum acceleration [m/ss] | | ||
| `normal.max_acc` | double | maximum acceleration [m/ss] | | ||
| `normal.min_jerk` | double | minimum jerk [m/sss] | | ||
| `normal.max_jerk` | double | maximum jerk [m/sss] | | ||
| `limit.min_acc` | double | minimum acceleration to be observed [m/ss] | | ||
| `limit.max_acc` | double | maximum acceleration to be observed [m/ss] | | ||
| `limit.min_jerk` | double | minimum jerk to be observed [m/sss] | | ||
| `limit.max_jerk` | double | maximum jerk to be observed [m/sss] | | ||
|
||
## Assumptions / Known limits | ||
|
||
<!-- Write assumptions and limitations of your implementation. | ||
Example: | ||
This algorithm assumes obstacles are not moving, so if they rapidly move after the vehicle started to avoid them, it might collide with them. | ||
Also, this algorithm doesn't care about blind spots. In general, since too close obstacles aren't visible due to the sensing performance limit, please take enough margin to obstacles. | ||
--> | ||
|
||
## (Optional) Error detection and handling | ||
|
||
<!-- Write how to detect errors and how to recover from them. | ||
Example: | ||
This package can handle up to 20 obstacles. If more obstacles found, this node will give up and raise diagnostic errors. | ||
--> | ||
|
||
## (Optional) Performance characterization | ||
|
||
<!-- Write performance information like complexity. If it wouldn't be the bottleneck, not necessary. | ||
Example: | ||
### Complexity | ||
This algorithm is O(N). | ||
### Processing time | ||
... | ||
--> | ||
|
||
## (Optional) References/External links | ||
|
||
<!-- Write links you referred to when you implemented. | ||
Example: | ||
[1] {link_to_a_thesis} | ||
[2] {link_to_an_issue} | ||
--> | ||
|
||
## (Optional) Future extensions / Unimplemented parts | ||
|
||
<!-- Write future extensions of this package. | ||
Example: | ||
Currently, this package can't handle the chattering obstacles well. We plan to add some probabilistic filters in the perception layer to improve it. | ||
Also, there are some parameters that should be global(e.g. vehicle size, max steering, etc.). These will be refactored and defined as global parameters so that we can share the same parameters between different nodes. | ||
--> |
4 changes: 4 additions & 0 deletions
4
planning/external_velocity_limit_selector/config/default.param.yaml
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 |
---|---|---|
@@ -0,0 +1,4 @@ | ||
/**: | ||
ros__parameters: | ||
# motion state constraints | ||
max_velocity: 20.0 # max velocity limit [m/s] |
15 changes: 15 additions & 0 deletions
15
planning/external_velocity_limit_selector/config/default_common.param.yaml
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 |
---|---|---|
@@ -0,0 +1,15 @@ | ||
/**: | ||
ros__parameters: | ||
# constraints param for normal driving | ||
normal: | ||
min_acc: -0.5 # min deceleration [m/ss] | ||
max_acc: 1.0 # max acceleration [m/ss] | ||
min_jerk: -0.5 # min jerk [m/sss] | ||
max_jerk: 1.0 # max jerk [m/sss] | ||
|
||
# constraints to be observed | ||
limit: | ||
min_acc: -2.5 # min deceleration limit [m/ss] | ||
max_acc: 1.0 # max acceleration limit [m/ss] | ||
min_jerk: -1.5 # min jerk limit [m/sss] | ||
max_jerk: 1.5 # max jerk limit [m/sss] |
Binary file added
BIN
+137 KB
...ing/external_velocity_limit_selector/image/external_velocity_limit_selector.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
75 changes: 75 additions & 0 deletions
75
...lector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.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
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,75 @@ | ||
// Copyright 2021 Tier IV, Inc. | ||
// | ||
// Licensed under the Apache License, Version 2.0 (the "License"); | ||
// you may not use this file except in compliance with the License. | ||
// You may obtain a copy of the License at | ||
// | ||
// http://www.apache.org/licenses/LICENSE-2.0 | ||
// | ||
// Unless required by applicable law or agreed to in writing, software | ||
// distributed under the License is distributed on an "AS IS" BASIS, | ||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
// See the License for the specific language governing permissions and | ||
// limitations under the License. | ||
|
||
#ifndef EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ | ||
#define EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ | ||
|
||
#include <rclcpp/rclcpp.hpp> | ||
|
||
#include <autoware_planning_msgs/msg/velocity_limit.hpp> | ||
#include <autoware_planning_msgs/msg/velocity_limit_clear_command.hpp> | ||
|
||
#include <deque> | ||
#include <memory> | ||
#include <string> | ||
#include <unordered_map> | ||
|
||
using autoware_planning_msgs::msg::VelocityLimit; | ||
using autoware_planning_msgs::msg::VelocityLimitClearCommand; | ||
using autoware_planning_msgs::msg::VelocityLimitConstraints; | ||
|
||
class ExternalVelocityLimitSelectorNode : public rclcpp::Node | ||
{ | ||
public: | ||
explicit ExternalVelocityLimitSelectorNode(const rclcpp::NodeOptions & node_options); | ||
|
||
void onVelocityLimitFromAPI(const VelocityLimit::ConstSharedPtr msg); | ||
void onVelocityLimitFromInternal(const VelocityLimit::ConstSharedPtr msg); | ||
void onVelocityLimitClearCommand(const VelocityLimitClearCommand::ConstSharedPtr msg); | ||
|
||
struct NodeParam | ||
{ | ||
double max_velocity; | ||
// constraints for normal driving | ||
double normal_min_acc; | ||
double normal_max_acc; | ||
double normal_min_jerk; | ||
double normal_max_jerk; | ||
// constraints to be observed | ||
double limit_min_acc; | ||
double limit_max_acc; | ||
double limit_min_jerk; | ||
double limit_max_jerk; | ||
}; | ||
|
||
private: | ||
rclcpp::Subscription<VelocityLimit>::SharedPtr sub_external_velocity_limit_from_api_; | ||
rclcpp::Subscription<VelocityLimit>::SharedPtr sub_external_velocity_limit_from_internal_; | ||
rclcpp::Subscription<VelocityLimitClearCommand>::SharedPtr sub_velocity_limit_clear_command_; | ||
rclcpp::Publisher<VelocityLimit>::SharedPtr pub_external_velocity_limit_; | ||
|
||
void publishVelocityLimit(const VelocityLimit & velocity_limit); | ||
void setVelocityLimitFromAPI(const VelocityLimit & velocity_limit); | ||
void setVelocityLimitFromInternal(const VelocityLimit & velocity_limit); | ||
void clearVelocityLimit(const std::string & sender); | ||
void updateVelocityLimit(); | ||
VelocityLimit getCurrentVelocityLimit() { return hardest_limit_; } | ||
|
||
// Parameters | ||
NodeParam node_param_{}; | ||
VelocityLimit hardest_limit_{}; | ||
std::unordered_map<std::string, VelocityLimit> velocity_limit_table_; | ||
}; | ||
|
||
#endif // EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ |
20 changes: 20 additions & 0 deletions
20
planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml
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 |
---|---|---|
@@ -0,0 +1,20 @@ | ||
<launch> | ||
<!-- common param --> | ||
<arg name="common_param_path" default="$(find-pkg-share external_velocity_limit_selector)/config/default_common.param.yaml"/> | ||
<arg name="param_path" default="$(find-pkg-share external_velocity_limit_selector)/config/default.param.yaml" /> | ||
|
||
<!-- input/output --> | ||
<arg name="input_velocity_limit_from_api" default="/planning/scenario_planning/max_velocity_default" /> | ||
<arg name="input_velocity_limit_from_internal" default="/planning/scenario_planning/max_velocity_candidates" /> | ||
<arg name="input_velocity_limit_clear_command_from_internal" default="/planning/scenario_planning/clear_velocity_limit" /> | ||
<arg name="output_velocity_limit_from_selector" default="/planning/scenario_planning/max_velocity" /> | ||
|
||
<node pkg="external_velocity_limit_selector" exec="external_velocity_limit_selector" name="external_velocity_limit_selector" output="screen"> | ||
<param from="$(var common_param_path)" /> | ||
<param from="$(var param_path)" /> | ||
<remap from="input/velocity_limit_from_api" to="$(var input_velocity_limit_from_api)"/> | ||
<remap from="input/velocity_limit_from_internal" to="$(var input_velocity_limit_from_internal)"/> | ||
<remap from="input/velocity_limit_clear_command_from_internal" to="$(var input_velocity_limit_clear_command_from_internal)"/> | ||
<remap from="output/external_velocity_limit" to="$(var output_velocity_limit_from_selector)"/> | ||
</node> | ||
</launch> |
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 |
---|---|---|
@@ -0,0 +1,25 @@ | ||
<?xml version="1.0"?> | ||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> | ||
<package format="3"> | ||
<name>external_velocity_limit_selector</name> | ||
<version>0.1.0</version> | ||
<description>The external_velocity_limit_selector ROS2 package</description> | ||
<maintainer email="[email protected]">Satoshi Ota</maintainer> | ||
<license>Apache License 2.0</license> | ||
|
||
<buildtool_depend>ament_cmake_auto</buildtool_depend> | ||
|
||
<depend>autoware_planning_msgs</depend> | ||
<depend>rclcpp</depend> | ||
<depend>rclcpp_components</depend> | ||
|
||
<exec_depend>ros2cli</exec_depend> | ||
<exec_depend>topic_tools</exec_depend> | ||
|
||
<test_depend>ament_lint_auto</test_depend> | ||
<test_depend>autoware_lint_common</test_depend> | ||
|
||
<export> | ||
<build_type>ament_cmake</build_type> | ||
</export> | ||
</package> |
Oops, something went wrong.