feat: add external_velocity_limit_selector package

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
+)

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
+
+<!-- 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.
+-->

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]

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]

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 <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_

planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml

@@ -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>

planning/external_velocity_limit_selector/package.xml

@@ -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="satoshi.ota@tier4.jp">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>