diff --git a/planning/external_velocity_limit_selector/CMakeLists.txt b/planning/external_velocity_limit_selector/CMakeLists.txt new file mode 100644 index 0000000000000..58cf571a6fc1a --- /dev/null +++ b/planning/external_velocity_limit_selector/CMakeLists.txt @@ -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 +) diff --git a/planning/external_velocity_limit_selector/README.md b/planning/external_velocity_limit_selector/README.md new file mode 100644 index 0000000000000..c79a04ede5523 --- /dev/null +++ b/planning/external_velocity_limit_selector/README.md @@ -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 + + + +## 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 + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts + + diff --git a/planning/external_velocity_limit_selector/config/default.param.yaml b/planning/external_velocity_limit_selector/config/default.param.yaml new file mode 100644 index 0000000000000..8023776e191ba --- /dev/null +++ b/planning/external_velocity_limit_selector/config/default.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + # motion state constraints + max_velocity: 20.0 # max velocity limit [m/s] diff --git a/planning/external_velocity_limit_selector/config/default_common.param.yaml b/planning/external_velocity_limit_selector/config/default_common.param.yaml new file mode 100644 index 0000000000000..a23570a5fc791 --- /dev/null +++ b/planning/external_velocity_limit_selector/config/default_common.param.yaml @@ -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] diff --git a/planning/external_velocity_limit_selector/image/external_velocity_limit_selector.png b/planning/external_velocity_limit_selector/image/external_velocity_limit_selector.png new file mode 100644 index 0000000000000..4139d916d1d00 Binary files /dev/null and b/planning/external_velocity_limit_selector/image/external_velocity_limit_selector.png differ diff --git a/planning/external_velocity_limit_selector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp b/planning/external_velocity_limit_selector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp new file mode 100644 index 0000000000000..e32ae24f50655 --- /dev/null +++ b/planning/external_velocity_limit_selector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp @@ -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 + +#include +#include + +#include +#include +#include +#include + +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::SharedPtr sub_external_velocity_limit_from_api_; + rclcpp::Subscription::SharedPtr sub_external_velocity_limit_from_internal_; + rclcpp::Subscription::SharedPtr sub_velocity_limit_clear_command_; + rclcpp::Publisher::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 velocity_limit_table_; +}; + +#endif // EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ diff --git a/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml b/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml new file mode 100644 index 0000000000000..b0bbd4595ce3f --- /dev/null +++ b/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/planning/external_velocity_limit_selector/package.xml b/planning/external_velocity_limit_selector/package.xml new file mode 100644 index 0000000000000..e1bd67a8d777b --- /dev/null +++ b/planning/external_velocity_limit_selector/package.xml @@ -0,0 +1,25 @@ + + + + external_velocity_limit_selector + 0.1.0 + The external_velocity_limit_selector ROS2 package + Satoshi Ota + Apache License 2.0 + + ament_cmake_auto + + autoware_planning_msgs + rclcpp + rclcpp_components + + ros2cli + topic_tools + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp b/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp new file mode 100644 index 0000000000000..ecf04cc448c3a --- /dev/null +++ b/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp @@ -0,0 +1,207 @@ +// 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. + +#include "external_velocity_limit_selector/external_velocity_limit_selector_node.hpp" + +#include +#include +#include +#include +#include +#include + +namespace +{ +VelocityLimit getHardestLimit( + const std::unordered_map & velocity_limits, + const ExternalVelocityLimitSelectorNode::NodeParam & node_param) +{ + VelocityLimit hardest_limit{}; + hardest_limit.max_velocity = node_param.max_velocity; + + VelocityLimitConstraints normal_constraints{}; + normal_constraints.min_acceleration = node_param.normal_min_acc; + normal_constraints.min_jerk = node_param.normal_min_jerk; + normal_constraints.max_jerk = node_param.normal_max_jerk; + + double hardest_max_velocity = node_param.max_velocity; + double hardest_max_jerk = 0.0; + + for (const auto & limit : velocity_limits) { + // guard nan, inf + const auto max_velocity = std::isfinite(limit.second.max_velocity) ? limit.second.max_velocity + : node_param.max_velocity; + + // find hardest max velocity + if (max_velocity < hardest_max_velocity) { + hardest_limit.stamp = limit.second.stamp; + hardest_limit.max_velocity = max_velocity; + hardest_max_velocity = max_velocity; + } + + const auto constraints = + limit.second.use_constraints && std::isfinite(limit.second.constraints.max_jerk) + ? limit.second.constraints + : normal_constraints; + + // find hardest jerk + if (hardest_max_jerk < constraints.max_jerk) { + hardest_limit.constraints = constraints; + hardest_limit.use_constraints = true; + hardest_max_jerk = constraints.max_jerk; + } + } + + return hardest_limit; +} +} // namespace + +ExternalVelocityLimitSelectorNode::ExternalVelocityLimitSelectorNode( + const rclcpp::NodeOptions & node_options) +: Node("external_velocity_limit_selector", node_options) +{ + using std::placeholders::_1; + // Input + sub_external_velocity_limit_from_api_ = this->create_subscription( + "input/velocity_limit_from_api", 1, + std::bind(&ExternalVelocityLimitSelectorNode::onVelocityLimitFromAPI, this, _1)); + + sub_external_velocity_limit_from_internal_ = this->create_subscription( + "input/velocity_limit_from_internal", 1, + std::bind(&ExternalVelocityLimitSelectorNode::onVelocityLimitFromInternal, this, _1)); + + sub_velocity_limit_clear_command_ = this->create_subscription( + "input/velocity_limit_clear_command_from_internal", 1, + std::bind(&ExternalVelocityLimitSelectorNode::onVelocityLimitClearCommand, this, _1)); + + // Output + pub_external_velocity_limit_ = + this->create_publisher("output/external_velocity_limit", 1); + + // Params + { + auto & p = node_param_; + p.max_velocity = this->declare_parameter("max_velocity", 20.0); + p.normal_min_acc = this->declare_parameter("normal.min_acc", -1.0); + p.normal_max_acc = this->declare_parameter("normal.max_acc", 1.0); + p.normal_min_jerk = this->declare_parameter("normal.min_jerk", -0.1); + p.normal_max_jerk = this->declare_parameter("normal.max_jerk", 0.1); + p.limit_min_acc = this->declare_parameter("limit.min_acc", -2.5); + p.limit_max_acc = this->declare_parameter("limit.max_acc", 2.5); + p.limit_min_jerk = this->declare_parameter("limit.min_jerk", -1.5); + p.limit_max_jerk = this->declare_parameter("limit.max_jerk", 1.5); + } +} + +void ExternalVelocityLimitSelectorNode::onVelocityLimitFromAPI( + const VelocityLimit::ConstSharedPtr msg) +{ + RCLCPP_DEBUG(get_logger(), "set velocity limit. sender:%s", msg->sender.c_str()); + setVelocityLimitFromAPI(*msg); + + const auto velocity_limit = getCurrentVelocityLimit(); + publishVelocityLimit(velocity_limit); +} + +void ExternalVelocityLimitSelectorNode::onVelocityLimitFromInternal( + const VelocityLimit::ConstSharedPtr msg) +{ + RCLCPP_DEBUG(get_logger(), "set velocity limit. sender:%s", msg->sender.c_str()); + setVelocityLimitFromInternal(*msg); + + const auto velocity_limit = getCurrentVelocityLimit(); + publishVelocityLimit(velocity_limit); +} + +void ExternalVelocityLimitSelectorNode::onVelocityLimitClearCommand( + const VelocityLimitClearCommand::ConstSharedPtr msg) +{ + if (!msg->command) { + return; + } + + clearVelocityLimit(msg->sender); + + const auto velocity_limit = getCurrentVelocityLimit(); + publishVelocityLimit(velocity_limit); +} + +void ExternalVelocityLimitSelectorNode::publishVelocityLimit(const VelocityLimit & velocity_limit) +{ + pub_external_velocity_limit_->publish(velocity_limit); +} + +void ExternalVelocityLimitSelectorNode::setVelocityLimitFromAPI( + const VelocityLimit & velocity_limit) +{ + const std::string sender = "api"; + + if (velocity_limit_table_.count(sender) == 0) { + velocity_limit_table_.emplace(sender, velocity_limit); + } else { + velocity_limit_table_.at(sender) = velocity_limit; + RCLCPP_DEBUG(get_logger(), "overwrite velocity limit. sender:%s", sender.c_str()); + } + + updateVelocityLimit(); +} + +void ExternalVelocityLimitSelectorNode::setVelocityLimitFromInternal( + const VelocityLimit & velocity_limit) +{ + const auto sender = velocity_limit.sender; + + if (velocity_limit_table_.count(sender) == 0) { + velocity_limit_table_.emplace(sender, velocity_limit); + } else { + velocity_limit_table_.at(sender) = velocity_limit; + RCLCPP_DEBUG(get_logger(), "overwrite velocity limit. sender:%s", sender.c_str()); + } + + updateVelocityLimit(); +} + +void ExternalVelocityLimitSelectorNode::clearVelocityLimit(const std::string & sender) +{ + if (velocity_limit_table_.empty()) { + RCLCPP_WARN(get_logger(), "no velocity limit has been set from internal."); + return; + } + + velocity_limit_table_.erase(sender); + + updateVelocityLimit(); +} + +void ExternalVelocityLimitSelectorNode::updateVelocityLimit() +{ + if (velocity_limit_table_.empty()) { + VelocityLimit default_velocity_limit{}; + default_velocity_limit.stamp = this->now(); + default_velocity_limit.max_velocity = node_param_.max_velocity; + + hardest_limit_ = default_velocity_limit; + + RCLCPP_DEBUG( + get_logger(), + "no velocity limit has been set or latest velocity limit has been already cleared."); + + return; + } + + hardest_limit_ = getHardestLimit(velocity_limit_table_, node_param_); +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(ExternalVelocityLimitSelectorNode)