From cf5905ddb87fa74a9ea9dc21f8751f4e11ca5f17 Mon Sep 17 00:00:00 2001 From: ktro2828 Date: Wed, 8 Jan 2025 11:54:10 +0900 Subject: [PATCH] chore: apply ktro2828/autoware_mtr Signed-off-by: ktro2828 --- perception/autoware_mtr/.gitignore | 51 ++ perception/autoware_mtr/.markdownlint.yaml | 16 + .../.pre-commit-config-optional.yaml | 6 + .../autoware_mtr/.pre-commit-config.yaml | 95 ++++ perception/autoware_mtr/CMakeLists.txt | 4 + perception/autoware_mtr/LICENSE | 1 - perception/autoware_mtr/README.md | 73 ++- perception/autoware_mtr/config/mtr.param.yaml | 4 - .../autoware_mtr/data/polyline_label.txt | 13 - .../include/autoware/mtr/agent.hpp | 266 +++++----- .../include/autoware/mtr/builder.hpp | 46 +- .../autoware/mtr/conversions/history.hpp | 62 +++ .../autoware/mtr/conversions/lanelet.hpp | 234 +++++++++ .../include/autoware/mtr/cuda_helper.hpp | 1 - .../include/autoware/mtr/fixed_queue.hpp | 87 ++++ .../include/autoware/mtr/intention_point.hpp | 65 +-- .../include/autoware/mtr/node.hpp | 94 ++-- .../include/autoware/mtr/polyline.hpp | 70 --- .../include/autoware/mtr/trajectory.hpp | 51 +- .../include/autoware/mtr/trt_mtr.hpp | 9 +- perception/autoware_mtr/launch/mtr.launch.xml | 2 +- .../attention/trt_attn_value_computation.hpp | 4 +- .../attention/trt_attn_weight_computation.hpp | 4 +- .../lib/include/common/trt_plugin_base.hpp | 4 +- .../lib/include/knn/trt_knn_batch.hpp | 4 +- .../lib/include/knn/trt_knn_batch_mlogk.hpp | 4 +- .../attention/trt_attn_value_computation.cpp | 6 +- .../attention/trt_attn_weight_computation.cpp | 6 +- .../lib/src/knn/trt_knn_batch.cpp | 6 +- .../lib/src/knn/trt_knn_batch_mlogk.cpp | 6 +- .../lib/src/postprocess/postprocess_kernel.cu | 4 +- .../preprocess/polyline_preprocess_kernel.cu | 87 ++-- perception/autoware_mtr/package.xml | 7 +- .../autoware_mtr/src/conversions/history.cpp | 111 ++++ .../autoware_mtr/src/conversions/lanelet.cpp | 142 +++++ .../autoware_mtr/src/intention_point.cpp | 41 ++ perception/autoware_mtr/src/node.cpp | 483 +++++++----------- perception/autoware_mtr/src/trt_mtr.cpp | 42 +- .../autoware_mtr/test/test_fixed_queue.cpp | 42 ++ 39 files changed, 1501 insertions(+), 752 deletions(-) create mode 100644 perception/autoware_mtr/.gitignore create mode 100644 perception/autoware_mtr/.markdownlint.yaml create mode 100644 perception/autoware_mtr/.pre-commit-config-optional.yaml create mode 100644 perception/autoware_mtr/.pre-commit-config.yaml delete mode 100644 perception/autoware_mtr/data/polyline_label.txt create mode 100644 perception/autoware_mtr/include/autoware/mtr/conversions/history.hpp create mode 100644 perception/autoware_mtr/include/autoware/mtr/conversions/lanelet.hpp create mode 100644 perception/autoware_mtr/include/autoware/mtr/fixed_queue.hpp create mode 100644 perception/autoware_mtr/src/conversions/history.cpp create mode 100644 perception/autoware_mtr/src/conversions/lanelet.cpp create mode 100644 perception/autoware_mtr/src/intention_point.cpp create mode 100644 perception/autoware_mtr/test/test_fixed_queue.cpp diff --git a/perception/autoware_mtr/.gitignore b/perception/autoware_mtr/.gitignore new file mode 100644 index 0000000000000..42bd33fbaa013 --- /dev/null +++ b/perception/autoware_mtr/.gitignore @@ -0,0 +1,51 @@ +devel/ +logs/ +build/ +bin/ +msg_gen/ +srv_gen/ +data/*onnx +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py +build_isolated/ +devel_isolated/ + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +# Catkin custom files +CATKIN_IGNORE diff --git a/perception/autoware_mtr/.markdownlint.yaml b/perception/autoware_mtr/.markdownlint.yaml new file mode 100644 index 0000000000000..584154b2009de --- /dev/null +++ b/perception/autoware_mtr/.markdownlint.yaml @@ -0,0 +1,16 @@ +# This file is automatically synced from: +# https://github.com/autowarefoundation/sync-file-templates +# To make changes, update the source repository and follow the guidelines in its README. + +# See https://github.com/DavidAnson/markdownlint/blob/main/doc/Rules.md for all rules. +default: true +MD013: false +MD024: + siblings_only: true +MD029: + style: ordered +MD033: false +MD041: false +MD045: false +MD046: false +MD049: false diff --git a/perception/autoware_mtr/.pre-commit-config-optional.yaml b/perception/autoware_mtr/.pre-commit-config-optional.yaml new file mode 100644 index 0000000000000..8c9345e15f064 --- /dev/null +++ b/perception/autoware_mtr/.pre-commit-config-optional.yaml @@ -0,0 +1,6 @@ +repos: + - repo: https://github.com/tcort/markdown-link-check + rev: v3.12.2 + hooks: + - id: markdown-link-check + args: [--quiet, --config=.markdown-link-check.json] diff --git a/perception/autoware_mtr/.pre-commit-config.yaml b/perception/autoware_mtr/.pre-commit-config.yaml new file mode 100644 index 0000000000000..63dc504f61a2b --- /dev/null +++ b/perception/autoware_mtr/.pre-commit-config.yaml @@ -0,0 +1,95 @@ +ci: + autofix_commit_msg: "style(pre-commit): autofix" + +repos: + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.6.0 + hooks: + - id: check-json + - id: check-merge-conflict + - id: check-toml + - id: check-xml + - id: check-yaml + args: [--unsafe] + - id: detect-private-key + - id: end-of-file-fixer + - id: mixed-line-ending + - id: trailing-whitespace + args: [--markdown-linebreak-ext=md] + + - repo: https://github.com/igorshubovych/markdownlint-cli + rev: v0.41.0 + hooks: + - id: markdownlint + args: [-c, .markdownlint.yaml, --fix] + + - repo: https://github.com/pre-commit/mirrors-prettier + rev: v4.0.0-alpha.8 + hooks: + - id: prettier + + - repo: https://github.com/adrienverge/yamllint + rev: v1.35.1 + hooks: + - id: yamllint + + - repo: https://github.com/tier4/pre-commit-hooks-ros + rev: v0.10.0 + hooks: + - id: flake8-ros + - id: prettier-xacro + - id: prettier-launch-xml + - id: prettier-package-xml + - id: ros-include-guard + - id: sort-package-xml + + - repo: https://github.com/shellcheck-py/shellcheck-py + rev: v0.10.0.1 + hooks: + - id: shellcheck + + - repo: https://github.com/scop/pre-commit-shfmt + rev: v3.9.0-1 + hooks: + - id: shfmt + args: [-w, -s, -i=4] + + - repo: https://github.com/pycqa/isort + rev: 5.13.2 + hooks: + - id: isort + + - repo: https://github.com/psf/black + rev: 24.8.0 + hooks: + - id: black + args: [--line-length=100] + + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v18.1.8 + hooks: + - id: clang-format + types_or: [c++, c, cuda] + + - repo: https://github.com/cpplint/cpplint + rev: 1.6.1 + hooks: + - id: cpplint + args: [--quiet] + exclude: .cu + + - repo: https://github.com/python-jsonschema/check-jsonschema + rev: 0.29.2 + hooks: + - id: check-metaschema + files: ^.+/schema/.*schema\.json$ + + - repo: local + hooks: + - id: prettier-svg + name: prettier svg + description: Apply Prettier with plugin-xml to svg. + entry: prettier --write --list-different --ignore-unknown --print-width 200 --xml-self-closing-space false --xml-whitespace-sensitivity ignore + language: node + files: .svg$ + additional_dependencies: [prettier@2.7.1, "@prettier/plugin-xml@2.2.0"] diff --git a/perception/autoware_mtr/CMakeLists.txt b/perception/autoware_mtr/CMakeLists.txt index b290902b04cc5..5dc5c2fb25106 100644 --- a/perception/autoware_mtr/CMakeLists.txt +++ b/perception/autoware_mtr/CMakeLists.txt @@ -55,6 +55,9 @@ target_include_directories(${PROJECT_NAME}_cuda_lib PUBLIC ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/builder.cpp src/trt_mtr.cpp + src/conversions/lanelet.cpp + src/intention_point.cpp + src/conversions/history.cpp ) target_link_libraries(${PROJECT_NAME}_lib ${TRT_PLUGINS} @@ -90,6 +93,7 @@ rclcpp_components_register_node(${PROJECT_NAME}_node if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + ament_auto_add_gtest(test_fixed_queue test/test_fixed_queue.cpp) endif() ament_auto_package( diff --git a/perception/autoware_mtr/LICENSE b/perception/autoware_mtr/LICENSE index d645695673349..261eeb9e9f8b2 100644 --- a/perception/autoware_mtr/LICENSE +++ b/perception/autoware_mtr/LICENSE @@ -1,4 +1,3 @@ - Apache License Version 2.0, January 2004 http://www.apache.org/licenses/ diff --git a/perception/autoware_mtr/README.md b/perception/autoware_mtr/README.md index 3a6238dfbc161..010fa8505dbdb 100644 --- a/perception/autoware_mtr/README.md +++ b/perception/autoware_mtr/README.md @@ -1 +1,72 @@ -# Motion TRansformer (MTR) +# autoware_mtr + +## Purpose + +The `autoware_mtr` package is used for 3D object motion prediction based on ML-based model called MTR. + +## Inner-workings / Algorithms + +The implementation bases on MTR [1] work. It uses TensorRT library for data process and network interface. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| -------------------- | ----------------------------------------------- | ------------------------ | +| `~/input/objects` | `autoware_perception_msgs::msg::TrackedObjects` | Input agent state. | +| `~/input/vector_map` | `autoware_map_msgs::msg::LeneletMapBin` | Input vector map. | +| `~/input/ego` | `sensor_msgs::msg::Odometry` | Input ego vehicle state. | + +### Output + +| Name | Type | Description | +| ------------------ | ------------------------------------------------- | -------------------------- | +| `~/output/objects` | `autoware_perception_msgs::msg::PredictedObjects` | Predicted objects' motion. | + +## Parameters + +Following parameters can be specified in `.launch.xml` or command line. + +### `param_path` + +File path to the MTR configuration. (Default: `autoware_mtr/config/mtr.param.yaml`) + +### Configuration Parameters + +#### `model_params` + +| Name | type | Description | +| :---------------------------- | :-----: | :---------------------------------------------------------- | +| `model_path` | `str` | ONNX or engine file path. | +| `target_labels` | `str[]` | An array of label names to be predicted. | +| `num_past` | `int` | The number of history length. | +| `num_mode` | `int` | The number of predicted modes. | +| `num_future` | `int` | The number of predicted future length. | +| `max_num_polyline` | `int` | The maximum number of polylines to be contained in input. | +| `max_num_point` | `int` | The maximum number of points included in a single polyline. | +| `point_break_distance` | `float` | Distance threshold to separate points into two polylines. | +| `intention_point_filepath` | `str` | File path to intension points (.csv). | +| `num_intention_point_cluster` | `int` | The number of clusters of intention points. | + +#### `build_params` + +| Name | type | Description | +| :----------- | :----: | :---------------------------------------------------------- | +| `is_dynamic` | `bool` | Indicates whether the model allows dynamic shape inference. | +| `precision` | `str` | Precision mode. | +| `MINMAX` | `str` | Calibration mode. | + +### `data_path` + +Directory path to ONNX or TensorRT engine file. (Default: `autoware_mtr/data`) + +### `build_only` + +This option performs to build the TensorRT engine file from the ONNX file and exit after finishing it. (Default: `false`) + +You can execute with the following command: + +```bash +ros2 launch autoware_mtr mtr.launch.xml build_only:=true +``` diff --git a/perception/autoware_mtr/config/mtr.param.yaml b/perception/autoware_mtr/config/mtr.param.yaml index 84851302c554a..932d544495d5b 100644 --- a/perception/autoware_mtr/config/mtr.param.yaml +++ b/perception/autoware_mtr/config/mtr.param.yaml @@ -11,10 +11,6 @@ point_break_distance: 1.0 intention_point_filepath: "$(var data_path)/intention_point.csv" num_intention_point_cluster: 64 - polyline_label_path: "$(var data_path)/polyline_label.txt" - static_inference: - num_target: 2 - num_agent: 7 build_params: is_dynamic: false precision: "FP32" diff --git a/perception/autoware_mtr/data/polyline_label.txt b/perception/autoware_mtr/data/polyline_label.txt deleted file mode 100644 index a66063c94dc61..0000000000000 --- a/perception/autoware_mtr/data/polyline_label.txt +++ /dev/null @@ -1,13 +0,0 @@ -road -highway -road_shoulder -bicycle_lane -pedestrian_lane -walkway -dashed -solid -dashed_dashed -road_boarder -crosswalk -traffic_sign -speed_bump diff --git a/perception/autoware_mtr/include/autoware/mtr/agent.hpp b/perception/autoware_mtr/include/autoware/mtr/agent.hpp index b977f8830ae80..797ed0d8b5e07 100644 --- a/perception/autoware_mtr/include/autoware/mtr/agent.hpp +++ b/perception/autoware_mtr/include/autoware/mtr/agent.hpp @@ -15,122 +15,127 @@ #ifndef AUTOWARE__MTR__AGENT_HPP_ #define AUTOWARE__MTR__AGENT_HPP_ +#include "autoware/mtr/fixed_queue.hpp" + +#include +#include + #include #include +#include #include +#include #include #include #include #include - namespace autoware::mtr { constexpr size_t AgentStateDim = 12; enum AgentLabel { VEHICLE = 0, PEDESTRIAN = 1, CYCLIST = 2 }; +enum AgentDimLabels { + X = 0, + Y = 1, + Z = 2, + L = 3, + W = 4, + H = 5, + YAW = 6, + VX = 7, + VY = 8, + AX = 9, + AY = 10, + VALIDITY = 11 +}; + /** * @brief A class to represent a single state of an agent. */ struct AgentState { // Construct a new instance filling all elements by `0.0f`. - AgentState() : data_({0.0f}) {} + AgentState() {} /** * @brief Construct a new instance with specified values. * - * @param x X position. - * @param y Y position. - * @param z Z position. - * @param length Length of the bbox. - * @param width Width of the bbox. - * @param height Height of the bbox. + * @param position 3D position [m]. + * @param dimension Box dimension [m]. * @param yaw Heading yaw angle [rad]. - * @param vx Velocity heading x direction in object's coordinates system. - * @param vy Velocity heading y direction in object's coordinates system. - * @param ax Acceleration heading x direction in object's coordinates system. - * @param ay Acceleration heading y direction in object's coordinates system. + * @param velocity Velocity [m/s]. + * @param acceleration Acceleration [m/s^2]. * @param is_valid `1.0f` if valid, otherwise `0.0f`. */ AgentState( - const float x, const float y, const float z, const float length, const float width, - const float height, const float yaw, const float vx, const float vy, const float ax, - const float ay, const float is_valid) - : data_({x, y, z, length, width, height, yaw, vx, vy, ax, ay, is_valid}), - x_(x), - y_(y), - z_(z), - length_(length), - width_(width), - height_(height), + const geometry_msgs::msg::Point & position, const geometry_msgs::msg::Vector3 & dimension, + float yaw, const geometry_msgs::msg::Vector3 & velocity, + const geometry_msgs::msg::Vector3 & acceleration, float is_valid) + : position_(position), + dimension_(dimension), yaw_(yaw), - vx_(vx), - vy_(vy), - ax_(ax), + velocity_(velocity), + acceleration_(acceleration), is_valid_(is_valid) { } - /** - * @brief Construct a new instance with a pointer. - * - * @param ptr - */ - explicit AgentState(const std::vector::const_iterator & itr) - { - std::copy(itr, itr + dim(), data_.begin()); - } - // Construct a new instance filling all elements by `0.0f`. static AgentState empty() noexcept { return AgentState(); } // Return the agent state dimensions `D`. static size_t dim() { return AgentStateDim; } - // Return the address pointer of data array. - const float * data_ptr() const noexcept { return data_.data(); } - // Return the x position. - float x() const { return x_; } + float x() const { return position_.x; } // Return the y position. - float y() const { return y_; } + float y() const { return position_.y; } // Return the z position. - float z() const { return z_; } + float z() const { return position_.z; } // Return the length of object size. - float length() const { return length_; } + float length() const { return dimension_.x; } // Return the width of object size. - float width() const { return width_; } + float width() const { return dimension_.y; } // Return the height of object size. - float height() const { return height_; } + float height() const { return dimension_.z; } // Return the yaw angle `[rad]`. float yaw() const { return yaw_; } // Return the x velocity. - float vx() const { return vx_; } + float vx() const { return velocity_.x; } // Return the y velocity. - float vy() const { return vy_; } + float vy() const { return velocity_.y; } // Return the x acceleration. - float ax() const { return ax_; } + float ax() const { return acceleration_.x; } // Return the y acceleration. - float ay() const { return ay_; } + float ay() const { return acceleration_.y; } // Return `true` if the value is `1.0`. - bool is_valid() const { return is_valid_ == 1.0f; } + bool is_valid() const { return is_valid_ == 1.0; } + + // Return the state attribute as an array. + std::array as_array() const noexcept + { + return {x(), y(), z(), length(), width(), height(), yaw(), vx(), vy(), ax(), ay(), is_valid_}; + } private: - std::array data_; - float x_{0.0f}, y_{0.0f}, z_{0.0f}, length_{0.0f}, width_{0.0f}, height_{0.0f}, yaw_{0.0f}, - vx_{0.0f}, vy_{0.0f}, ax_{0.0f}, ay_{0.0f}, is_valid_{0.0f}; + geometry_msgs::msg::Point position_; + geometry_msgs::msg::Vector3 dimension_; + float yaw_{0.0f}; + geometry_msgs::msg::Vector3 velocity_; + geometry_msgs::msg::Vector3 acceleration_; + float is_valid_{0.0f}; }; /** @@ -143,38 +148,20 @@ struct AgentHistory * * @param state Object current state. * @param object_id Object ID. + * @param label_id Label ID. * @param current_time Current timestamp. * @param max_time_length History length. */ AgentHistory( - const AgentState & state, const std::string & object_id, const size_t label_index, - const float current_time, const size_t max_time_length) - : data_((max_time_length - 1) * state_dim()), + const AgentState & state, const std::string & object_id, const size_t label_id, + const double current_time, const size_t max_time_length) + : queue_(max_time_length), object_id_(object_id), - label_index_(label_index), + label_id_(label_id), latest_time_(current_time), max_time_length_(max_time_length) { - const auto s_ptr = state.data_ptr(); - for (size_t d = 0; d < state_dim(); ++d) { - data_.push_back(*(s_ptr + d)); - } - } - - /** - * @brief Construct a new Agent History filling all elements by zero. - * - * @param object_id Object ID. - * @param max_time_length History time length. - */ - AgentHistory( - const std::string & object_id, const size_t label_index, const size_t max_time_length) - : data_(max_time_length * state_dim()), - object_id_(object_id), - label_index_(label_index), - latest_time_(-std::numeric_limits::max()), - max_time_length_(max_time_length) - { + queue_.push_back(state); } // Return the history time length `T`. @@ -192,14 +179,15 @@ struct AgentHistory // Return the object id. const std::string & object_id() const { return object_id_; } - size_t label_index() const { return label_index_; } + // Return the label id. + size_t label_id() const { return label_id_; } /** * @brief Return the last timestamp when non-empty state was pushed. * - * @return float + * @return double */ - float latest_time() const { return latest_time_; } + double latest_time() const { return latest_time_; } /** * @brief Update history with input state and latest time. @@ -207,33 +195,31 @@ struct AgentHistory * @param current_time The current timestamp. * @param state The current agent state. */ - void update(const float current_time, AgentState & state) noexcept + void update(double current_time, const AgentState & state) noexcept { - // remove the state at the oldest timestamp - data_.erase(data_.begin(), data_.begin() + state_dim()); - - const auto s = state.data_ptr(); - for (size_t d = 0; d < state_dim(); ++d) { - data_.push_back(*(s + d)); - } + queue_.push_back(state); latest_time_ = current_time; } // Update history with all-zeros state, but latest time is not updated. void update_empty() noexcept { - // remove the state at the oldest timestamp - data_.erase(data_.begin(), data_.begin() + state_dim()); + const auto state = AgentState::empty(); + queue_.push_back(state); + } - const auto s = AgentState::empty().data_ptr(); - for (size_t d = 0; d < state_dim(); ++d) { - data_.push_back(*(s + d)); + // Return a history states as an array. + std::vector as_array() const noexcept + { + std::vector output; + for (const auto & state : queue_) { + for (const auto & v : state.as_array()) { + output.push_back(v); + } } + return output; } - // Return the address pointer of data array. - const float * data_ptr() const noexcept { return data_.data(); } - /** * @brief Check whether the latest valid state is too old or not. * @@ -242,7 +228,7 @@ struct AgentHistory * @return true If the difference is greater than threshold. * @return false Otherwise */ - bool is_ancient(const float current_time, const float threshold) const + bool is_ancient(double current_time, double threshold) const { /* TODO: Raise error if the current time is smaller than latest */ return current_time - latest_time_ >= threshold; @@ -257,17 +243,22 @@ struct AgentHistory bool is_valid_latest() const { return get_latest_state().is_valid(); } // Get the latest agent state at `T`. - AgentState get_latest_state() const + const AgentState & get_latest_state() const { return queue_.back(); } + + // Get the latest agent state at `T`. + std::optional get_latest_valid_state() const { - const auto & latest_itr = (data_.begin() + state_dim() * (max_time_length_ - 1)); - return AgentState(latest_itr); + auto latest_valid_state = std::find_if( + queue_.rbegin(), queue_.rend(), [](const auto & state) { return state.is_valid(); }); + return (latest_valid_state != queue_.rend()) ? std::make_optional(*latest_valid_state) + : std::nullopt; } private: - std::vector data_; + FixedQueue queue_; const std::string object_id_; - const size_t label_index_; - float latest_time_; + const size_t label_id_; + double latest_time_; const size_t max_time_length_; }; @@ -280,49 +271,42 @@ struct AgentData * @brief Construct a new instance. * * @param histories An array of histories for each object. - * @param sdc_index An index of ego. - * @param target_index Indices of target agents. - * @param label_index An array of label indices for each object. + * @param ego_index An index of ego. + * @param target_indices Indices of target agents. + * @param label_ids An array of label indices for each object. * @param timestamps An array of timestamps. */ AgentData( - const std::vector & histories, const size_t sdc_index, - const std::vector & target_index, const std::vector & label_index, + const std::vector & histories, const size_t ego_index, + const std::vector & target_indices, const std::vector & label_ids, const std::vector & timestamps) - : num_target_(target_index.size()), + : num_target_(target_indices.size()), num_agent_(histories.size()), time_length_(timestamps.size()), - sdc_index_(sdc_index), - target_index_(target_index), - label_index_(label_index), + ego_index_(ego_index), + target_indices_(target_indices), + label_ids_(label_ids), timestamps_(timestamps) { data_.reserve(num_agent_ * time_length_ * state_dim()); for (auto & history : histories) { - const auto data_ptr = history.data_ptr(); - for (size_t t = 0; t < time_length_; ++t) { - for (size_t d = 0; d < state_dim(); ++d) { - data_.push_back(*(data_ptr + t * state_dim() + d)); - } + for (const auto & v : history.as_array()) { + data_.push_back(v); } } - target_data_.reserve(num_target_ * state_dim()); - target_label_index_.reserve(num_target_); - for (const auto & idx : target_index) { - target_label_index_.emplace_back(label_index.at(idx)); - const auto target_ptr = histories.at(idx).data_ptr(); - for (size_t d = 0; d < state_dim(); ++d) { - target_data_.push_back(*(target_ptr + (time_length_ - 1) * state_dim() + d)); + current_target_data_.reserve(num_target_ * state_dim()); // (B, D) + target_label_ids_.reserve(num_target_); + for (const auto & idx : target_indices) { + target_label_ids_.emplace_back(label_ids.at(idx)); + for (const auto & v : histories.at(idx).get_latest_state().as_array()) { + current_target_data_.push_back(v); } } ego_data_.reserve(time_length_ * state_dim()); - const auto ego_data_ptr = histories.at(sdc_index).data_ptr(); - for (size_t t = 0; t < time_length_; ++t) { - for (size_t d = 0; d < state_dim(); ++d) { - ego_data_.push_back(*(ego_data_ptr + t * state_dim() + d)); - } + for (const auto & v : histories.at(ego_index).as_array()) { + ego_data_.push_back(v); } } @@ -342,16 +326,16 @@ struct AgentData static size_t state_dim() { return AgentStateDim; } // Return the index of ego. - int sdc_index() const { return sdc_index_; } + size_t ego_index() const { return ego_index_; } // Return the vector of indices of target agents, in shape `[B]`. - const std::vector & target_index() const { return target_index_; } + const std::vector & target_indices() const { return target_indices_; } - // Return the vector of label indices of all agents, in shape `[N]`. - const std::vector & label_index() const { return label_index_; } + // Return the vector of label ids of all agents, in shape `[N]`. + const std::vector & label_ids() const { return label_ids_; } - // Return the vector of label indices of target agents, in shape `[B]`. - const std::vector & target_label_index() const { return target_label_index_; } + // Return the vector of label ids of target agents, in shape `[B]`. + const std::vector & target_label_ids() const { return target_label_ids_; } // Return the vector of timestamps in shape `[T]`. const std::vector & timestamps() const { return timestamps_; } @@ -372,7 +356,7 @@ struct AgentData const float * data_ptr() const noexcept { return data_.data(); } // Return the address pointer of data array for target agents. - const float * target_data_ptr() const noexcept { return target_data_.data(); } + const float * current_target_data_ptr() const noexcept { return current_target_data_.data(); } // Return the address pointer of data array for ego vehicle. const float * ego_data_ptr() const noexcept { return ego_data_.data(); } @@ -381,18 +365,18 @@ struct AgentData size_t num_target_; size_t num_agent_; size_t time_length_; - int sdc_index_; - std::vector target_index_; - std::vector label_index_; - std::vector target_label_index_; + size_t ego_index_; + std::vector target_indices_; + std::vector label_ids_; + std::vector target_label_ids_; std::vector timestamps_; std::vector data_; - std::vector target_data_; + std::vector current_target_data_; std::vector ego_data_; }; // Get label names from label indices. -std::vector getLabelNames(const std::vector & label_index) +inline std::vector getLabelNames(const std::vector & label_index) { std::vector label_names; label_names.reserve(label_index.size()); diff --git a/perception/autoware_mtr/include/autoware/mtr/builder.hpp b/perception/autoware_mtr/include/autoware/mtr/builder.hpp index e93be26eca0be..7dbe0895caa8d 100644 --- a/perception/autoware_mtr/include/autoware/mtr/builder.hpp +++ b/perception/autoware_mtr/include/autoware/mtr/builder.hpp @@ -54,9 +54,42 @@ using TrtUniquePtr = std::unique_ptr>; // Type names of precisions. enum PrecisionType { FP32 = 0, FP16 = 1, INT8 = 2 }; +// Return corresponding PrecisionType from string. +inline PrecisionType toPrecisionType(const std::string & name) +{ + if (name == "FP32") { + return PrecisionType::FP32; + } + if (name == "FP16") { + return PrecisionType::FP16; + } + if (name == "INT8") { + return PrecisionType::INT8; + } + throw std::invalid_argument("Invalid precision name."); +} + // Type names of calibrations. enum CalibrationType { ENTROPY = 0, LEGACY = 1, PERCENTILE = 2, MINMAX = 3 }; +// Return corresponding CalibrationType from string. +inline CalibrationType toCalibrationType(const std::string & name) +{ + if (name == "ENTROPY") { + return CalibrationType::ENTROPY; + } + if (name == "LEGACY") { + return CalibrationType::LEGACY; + } + if (name == "PERCENTILE") { + return CalibrationType::PERCENTILE; + } + if (name == "MINMAX") { + return CalibrationType::MINMAX; + } + throw std::invalid_argument("Invalid calibration name."); +} + struct BatchOptConfig { /** @@ -112,12 +145,11 @@ struct BuildConfig * @param calibration */ BuildConfig( - const bool is_dynamic, const PrecisionType & precision = PrecisionType::FP32, - const CalibrationType & calibration = CalibrationType::MINMAX, + const bool is_dynamic, const std::string & precision, const std::string & calibration, const BatchOptConfig & batch_target = BatchOptConfig(1, 10, 20), const BatchOptConfig & batch_agent = BatchOptConfig(1, 30, 50)) - : precision(precision), - calibration(calibration), + : precision(toPrecisionType(precision)), + calibration(toCalibrationType(calibration)), batch_target(batch_target), batch_agent(batch_agent), is_dynamic_(is_dynamic) @@ -155,10 +187,10 @@ class MTRBuilder * * @return True if plugins were initialized successfully. */ - bool isInitialized() const; + [[nodiscard]] bool isInitialized() const; // Return true if the model supports dynamic shape inference. - bool isDynamic() const; + [[nodiscard]] bool isDynamic() const; // Set binding dimensions for specified for dynamic shape inference. bool setBindingDimensions(int index, nvinfer1::Dims dimensions); @@ -184,7 +216,7 @@ class MTRBuilder bool loadEngine(const std::string & filepath); // Create a cache path of engine file. - fs::path createEngineCachePath() const; + [[nodiscard]] fs::path createEngineCachePath() const; /** * @brief Build engine from onnx file. diff --git a/perception/autoware_mtr/include/autoware/mtr/conversions/history.hpp b/perception/autoware_mtr/include/autoware/mtr/conversions/history.hpp new file mode 100644 index 0000000000000..56d6bcb06a712 --- /dev/null +++ b/perception/autoware_mtr/include/autoware/mtr/conversions/history.hpp @@ -0,0 +1,62 @@ +// Copyright 2024 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 AUTOWARE__MTR__CONVERSIONS__HISTORY_HPP_ +#define AUTOWARE__MTR__CONVERSIONS__HISTORY_HPP_ + +#include "autoware/mtr/agent.hpp" + +#include +#include +#include +#include +#include +#include + +namespace autoware::mtr +{ + +/** + * @brief Transform a 2D point with a reference point and rotation + * + * @param history history to modify. + * @param reference_state reference state used as reference frame. + * @return vector of histories, each in the reference frame of the last state. + */ +[[nodiscard]] std::pair transform2D( + const std::pair input_xy, const std::pair reference_xy, + const std::pair rotation_cos_sin); + +/** + * @brief Get histories with agent states in the reference frame of the last state + * + * @param history history to modify. + * @param reference_state reference state used as reference frame. + * @return vector of histories, each in the reference frame of the last state. + */ +[[nodiscard]] AgentHistory getAgentCentricHistory( + const AgentHistory & history, const AgentState & reference_state); + +/** + * @brief Get histories with agent states in the reference frame of the last state + * + * @param histories vector of histories to modify. + * @return vector of histories, each in the reference frame of the last state. + */ +[[nodiscard]] std::vector getAgentCentricHistories( + const std::vector & histories); + +} // namespace autoware::mtr + +#endif // AUTOWARE__MTR__CONVERSIONS__HISTORY_HPP_ diff --git a/perception/autoware_mtr/include/autoware/mtr/conversions/lanelet.hpp b/perception/autoware_mtr/include/autoware/mtr/conversions/lanelet.hpp new file mode 100644 index 0000000000000..17742d333acca --- /dev/null +++ b/perception/autoware_mtr/include/autoware/mtr/conversions/lanelet.hpp @@ -0,0 +1,234 @@ +// Copyright 2024 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 AUTOWARE__MTR__CONVERSIONS__LANELET_HPP_ +#define AUTOWARE__MTR__CONVERSIONS__LANELET_HPP_ + +#include "autoware/mtr/polyline.hpp" + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware::mtr +{ +/** + * @brief Insert lane points into the container from the end of it. + * + * @param points Sequence of points to be inserted. + * @param container Points container. + */ +inline void insertLanePoints( + const std::vector & points, std::vector & container) +{ + container.reserve(container.size() * 2); + container.insert(container.end(), points.begin(), points.end()); +} + +inline lanelet::Optional toTypeName(const lanelet::ConstLanelet & lanelet) +{ + return lanelet.hasAttribute("type") ? lanelet.attribute("type").as() + : lanelet::Optional(); +} + +inline lanelet::Optional toTypeName(const lanelet::ConstLineString3d & linestring) +{ + return linestring.hasAttribute("type") ? linestring.attribute("type").as() + : lanelet::Optional(); +} + +/** + * @brief Extract the subtype name from a lanelet. + * + * @param lanelet Lanelet instance. + * @return std::optional + */ +inline lanelet::Optional toSubtypeName(const lanelet::ConstLanelet & lanelet) noexcept +{ + return lanelet.hasAttribute("subtype") ? lanelet.attribute("subtype").as() + : lanelet::Optional(); +} + +/** + * @brief Extract the subtype name from a 3D linestring. + * + * @param linestring 3D linestring instance. + * @return lanelet::Optional + */ +inline lanelet::Optional toSubtypeName( + const lanelet::ConstLineString3d & linestring) noexcept +{ + return linestring.hasAttribute("subtype") ? linestring.attribute("subtype").as() + : lanelet::Optional(); +} + +/** + * @brief Check if the specified lanelet is the turnable intersection. + * + * @param lanelet Lanelet instance. + * @return true if the lanelet has the attribute named turn_direction. + */ +inline bool isTurnableIntersection(const lanelet::ConstLanelet & lanelet) noexcept +{ + return lanelet.hasAttribute("turn_direction"); +} + +/** + * @brief Check if the specified lanelet subtype is kind of lane. + * + * @param subtype + * @return True if the lanelet subtype is the one of the (road, highway, road_shoulder, + * pedestrian_lane, bicycle_lane, walkway). + */ +inline bool isLaneLike(const lanelet::Optional & subtype) +{ + if (!subtype) { + return false; + } + const auto & subtype_str = subtype.value(); + return ( + subtype_str == "road" || subtype_str == "highway" || subtype_str == "road_shoulder" || + subtype_str == "pedestrian_lane" || subtype_str == "bicycle_lane" || subtype_str == "walkway"); +} + +/** + * @brief Check if the specified lanelet subtype is kind of the roadway. + * + * @param subtype Subtype of the corresponding lanelet. + * @return True if the subtype is the one of the (road, highway, road_shoulder). + */ +inline bool isRoadwayLike(const lanelet::Optional & subtype) +{ + if (!subtype) { + return false; + } + const auto & subtype_str = subtype.value(); + return subtype_str == "road" || subtype_str == "highway" || subtype_str == "road_shoulder"; +} + +/** + * @brief Check if the specified linestring is kind of the boundary. + * + * @param linestring 3D linestring. + * @return True if the type is the one of the (line_thin, line_thick, road_boarder) and the subtype + * is not virtual. + */ +inline bool isBoundaryLike(const lanelet::ConstLineString3d & linestring) +{ + const auto type = toTypeName(linestring); + const auto subtype = toSubtypeName(linestring); + if (!type || !subtype) { + return false; + } + + const auto & type_str = type.value(); + const auto & subtype_str = subtype.value(); + return (type_str == "line_thin" || type_str == "line_thick" || type_str == "road_boarder") && + subtype_str != "virtual"; +} + +/** + * @brief Check if the specified linestring is the kind of crosswalk. + * + * @param subtype Subtype of the corresponding polygon. + * @return True if the lanelet subtype is the one of the (crosswalk,). + */ +inline bool isCrosswalkLike(const lanelet::Optional & subtype) +{ + if (!subtype) { + return false; + } + + const auto & subtype_str = subtype.value(); + return subtype_str == "crosswalk"; +} + +/** + * @brief A class to convert lanelet map to polyline. + */ +class LaneletConverter +{ +public: + /** + * @brief Construct a new Lanelet Converter object + * + * @param lanelet_map_ptr Pointer of loaded lanelet map. + * @param max_num_polyline The max number of polylines to be contained in the tensor. If the total + * number of polylines are less than this value, zero-filled polylines will be padded. + * @param max_num_point The max number of points to be contained in a single polyline. + * @param point_break_distance Distance threshold to separate two polylines. + */ + explicit LaneletConverter( + lanelet::LaneletMapConstPtr lanelet_map_ptr, size_t max_num_polyline, size_t max_num_point, + float point_break_distance) + : lanelet_map_ptr_(lanelet_map_ptr), + max_num_polyline_(max_num_polyline), + max_num_point_(max_num_point), + point_break_distance_(point_break_distance) + { + } + + /** + * @brief Convert a lanelet map to the polyline data except of points whose distance from the + * specified position is farther than the threshold. + * + * @param position Origin to check the distance from this. + * @param distance_threshold Distance threshold + * @return std::optional + */ + std::optional convert( + const geometry_msgs::msg::Point & position, double distance_threshold) const; + +private: + /** + * @brief Convert a linestring to the set of polylines. + * + * @param linestring Linestring instance. + * @param position Origin to check the distance from this. + * @param distance_threshold Distance threshold from the specified position. + * @return std::vector + */ + std::vector fromLinestring( + const lanelet::ConstLineString3d & linestring, const geometry_msgs::msg::Point & position, + double distance_threshold) const noexcept; + + /** + * @brief Convert a polygon to the set of polylines. + * + * @param polygon Polygon instance. + * @param position Origin to check the distance from this. + * @param distance_threshold Distance threshold from the specified position. + * @return std::vector + */ + std::vector fromPolygon( + const lanelet::CompoundPolygon3d & polygon, const geometry_msgs::msg::Point & position, + double distance_threshold) const noexcept; + + lanelet::LaneletMapConstPtr lanelet_map_ptr_; //!< Pointer of lanelet map. + size_t max_num_polyline_; //!< The max number of polylines. + size_t max_num_point_; //!< The max number of points. + float point_break_distance_; //!< Distance threshold to separate two polylines. +}; +} // namespace autoware::mtr + +#endif // AUTOWARE__MTR__CONVERSIONS__LANELET_HPP_ diff --git a/perception/autoware_mtr/include/autoware/mtr/cuda_helper.hpp b/perception/autoware_mtr/include/autoware/mtr/cuda_helper.hpp index b73d305b32c6b..468b6b61d3352 100644 --- a/perception/autoware_mtr/include/autoware/mtr/cuda_helper.hpp +++ b/perception/autoware_mtr/include/autoware/mtr/cuda_helper.hpp @@ -23,7 +23,6 @@ #include -#include #include #include #include diff --git a/perception/autoware_mtr/include/autoware/mtr/fixed_queue.hpp b/perception/autoware_mtr/include/autoware/mtr/fixed_queue.hpp new file mode 100644 index 0000000000000..8645628848792 --- /dev/null +++ b/perception/autoware_mtr/include/autoware/mtr/fixed_queue.hpp @@ -0,0 +1,87 @@ +// Copyright 2024 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 AUTOWARE__MTR__FIXED_QUEUE_HPP_ +#define AUTOWARE__MTR__FIXED_QUEUE_HPP_ + +#include +#include +#include + +namespace autoware::mtr +{ + +template +class FixedQueue +{ +public: + using size_type = typename std::deque::size_type; + using reference = typename std::deque::reference; + using const_reference = typename std::deque::const_reference; + using iterator = typename std::deque::iterator; + using riterator = typename std::reverse_iterator; + using const_iterator = typename std::deque::const_iterator; + using rconst_iterator = typename std::reverse_iterator; + + explicit FixedQueue(size_type size) : queue_(size) {} + + void push_back(const T && t) noexcept + { + queue_.pop_front(); + queue_.push_back(t); + } + + void push_back(const T & t) noexcept + { + queue_.pop_front(); + queue_.push_back(t); + } + + void push_front(const T && t) noexcept + { + queue_.pop_back(); + queue_.push_front(t); + } + + void push_front(const T & t) noexcept + { + queue_.pop_back(); + queue_.push_front(t); + } + + reference front() noexcept { return queue_.front(); } + const_reference front() const noexcept { return queue_.front(); } + + reference back() noexcept { return queue_.back(); } + const_reference back() const noexcept { return queue_.back(); } + + iterator begin() noexcept { return queue_.begin(); } + const_iterator begin() const noexcept { return queue_.begin(); } + + iterator end() noexcept { return queue_.end(); } + const_iterator end() const noexcept { return queue_.end(); } + + riterator rbegin() noexcept { return queue_.rbegin(); } + rconst_iterator rbegin() const noexcept { return queue_.rbegin(); } + + riterator rend() noexcept { return queue_.rend(); } + rconst_iterator rend() const noexcept { return queue_.rend(); } + + size_type size() const noexcept { return queue_.size(); } + +private: + std::deque queue_; +}; +} // namespace autoware::mtr +#endif // AUTOWARE__MTR__FIXED_QUEUE_HPP_ diff --git a/perception/autoware_mtr/include/autoware/mtr/intention_point.hpp b/perception/autoware_mtr/include/autoware/mtr/intention_point.hpp index 5fb35e7b1fb6e..0cdf1c739dc3d 100644 --- a/perception/autoware_mtr/include/autoware/mtr/intention_point.hpp +++ b/perception/autoware_mtr/include/autoware/mtr/intention_point.hpp @@ -26,9 +26,6 @@ namespace autoware::mtr { - -constexpr size_t IntentionPointDim = 2; - /** * @brief A class to load and store intention points. */ @@ -36,27 +33,12 @@ struct IntentionPoint { public: /** - * @brief Construct a new Intention. - * - * @param data_map Map of intention points hashed by label names. - * @param num_cluster The number of clusters. - */ - IntentionPoint( - const std::unordered_map> data_map, const size_t num_cluster) - : data_map_(data_map), num_cluster_(num_cluster) - { - } - - static size_t state_dim() { return IntentionPointDim; } - - /** - * @brief Construct a new Intention from csv file. + * @brief Construct a new instance from a csv file. * * @param csv_filepath Path to csv file. * @param num_cluster The number of clusters. */ - IntentionPoint(const std::string csv_filepath, const size_t num_cluster) - : num_cluster_(num_cluster) + static IntentionPoint from_file(size_t num_cluster, const std::string csv_filepath) { std::ifstream file(csv_filepath); if (!file.is_open()) { @@ -82,40 +64,45 @@ struct IntentionPoint } file.close(); + std::unordered_map> data_map; for (const auto & [x, y, label] : buffer) { - data_map_[label].emplace_back(x); - data_map_[label].emplace_back(y); + data_map[label].emplace_back(x); + data_map[label].emplace_back(y); } + + return {num_cluster, data_map}; } + // Returns the point state dimension, which is 2 (x, y). + static size_t state_dim() noexcept { return 2; } + /** * @brief Load intention points for specified label names. * * @param label_names Array of label names for all agents, in shape [N]. * @return std::vector Array of all points in shape, [N * num_cluster * 2]. */ - std::vector get_points(const std::vector & label_names) - { - std::vector points; - points.reserve(label_names.size() * num_cluster_ * state_dim()); - for (const auto & name : label_names) { - const auto & label_points = data_map_.at(name); - for (const auto & p : label_points) { - points.emplace_back(p); - } - } - return points; - } + std::vector as_array(const std::vector & label_names) const; // Return the size of intension point `K*D`. - size_t size() const { return num_cluster_ * state_dim(); } + size_t size() const noexcept; - // Return the number of clusters contained in intention points `K`. - size_t num_cluster() const { return num_cluster_; } + size_t num_cluster; //!< The number of point clusters, which is K. private: - std::unordered_map> data_map_; - size_t num_cluster_; + /** + * @brief Construct a new Intention. + * + * @param data_map Map of intention points hashed by label names. + * @param num_cluster The number of clusters. + */ + IntentionPoint( + size_t num_cluster, const std::unordered_map> data_map) + : num_cluster(num_cluster), data_map_(data_map) + { + } + + std::unordered_map> data_map_; //!< Map of label name and points. }; } // namespace autoware::mtr #endif // AUTOWARE__MTR__INTENTION_POINT_HPP_ diff --git a/perception/autoware_mtr/include/autoware/mtr/node.hpp b/perception/autoware_mtr/include/autoware/mtr/node.hpp index 45fd7c827a283..4feaf0fe534a0 100644 --- a/perception/autoware_mtr/include/autoware/mtr/node.hpp +++ b/perception/autoware_mtr/include/autoware/mtr/node.hpp @@ -16,17 +16,23 @@ #define AUTOWARE__MTR__NODE_HPP_ #include "autoware/mtr/agent.hpp" +#include "autoware/mtr/conversions/lanelet.hpp" +#include "autoware/mtr/fixed_queue.hpp" #include "autoware/mtr/polyline.hpp" #include "autoware/mtr/trajectory.hpp" #include "autoware/mtr/trt_mtr.hpp" #include #include +#include #include #include +#include #include +#include #include +#include #include #include #include @@ -41,13 +47,15 @@ #include #include #include +#include #include #include #include namespace autoware::mtr { -using autoware_map_msgs::msg::LaneletMapBin; +using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; +using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjectKinematics; @@ -57,106 +65,67 @@ using autoware_perception_msgs::msg::TrackedObject; using autoware_perception_msgs::msg::TrackedObjects; using nav_msgs::msg::Odometry; -// TODO(ktro2828): use received ego size topic -// wheel_base: between front wheel center and rear wheel center [m] -// wheel_tread: between left wheel center and right wheel center [m] -// front_overhang: between front wheel center and vehicle front [m] -// rear_overhang: between rear wheel center and vehicle rear [m] -// left_overhang: between left wheel center and vehicle left [m] -// right_overhang: between right wheel center and vehicle right [m] -constexpr float EGO_LENGTH = 4.0f; -constexpr float EGO_WIDTH = 2.0f; -constexpr float EGO_HEIGHT = 1.0f; - -class PolylineTypeMap -{ -public: - explicit PolylineTypeMap(rclcpp::Node * node) - { - const auto filepath = node->declare_parameter("model_params.polyline_label_path"); - std::ifstream file(filepath); - if (!file.is_open()) { - RCLCPP_ERROR_STREAM(node->get_logger(), "Could not open polyline label file: " << filepath); - rclcpp::shutdown(); - } - - int label_index = 0; - std::string label; - while (getline(file, label)) { - label_map_.insert({label, label_index}); - ++label_index; - } - } - - // Return the ID corresponding to the label type. If specified type is not contained in map, - // return `-1`. - int type_to_id(const std::string & type) const - { - return label_map_.count(type) == 0 ? -1 : label_map_.at(type); - } - -private: - std::map label_map_; -}; // class PolylineTypeMap - class MTRNode : public rclcpp::Node { public: explicit MTRNode(const rclcpp::NodeOptions & node_options); // Object ID of the ego vehicle - const std::string EGO_ID{"EGO"}; + inline static const std::string EGO_ID{"EGO"}; private: // Main callback being invoked when the tracked objects topic is subscribed. void callback(const TrackedObjects::ConstSharedPtr object_msg); // Callback being invoked when the HD map topic is subscribed. - void on_map(const LaneletMapBin::ConstSharedPtr map_msg); + void onMap(const HADMapBin::ConstSharedPtr map_msg); - // Callback being invoked when the Ego's odometry topic is subscribed. - void on_ego(const Odometry::ConstSharedPtr ego_msg); + // Fetch data of Ego's odometry topic. + std::optional getLatestEgo(); // Convert Lanelet to `PolylineData`. - bool lanelet_to_polyline(); + bool convertLaneletToPolyline(); // Remove ancient agent histories. - void remove_ancient_history( - const float current_time, const TrackedObjects::ConstSharedPtr objects_msg); + void removeAncientAgentHistory( + const double current_time, const TrackedObjects::ConstSharedPtr objects_msg); // Appends new states to history. - void update_history(const float current_time, const TrackedObjects::ConstSharedPtr objects_msg); - - // Extract ego state stored in the buffer which has the nearest timestamp from current timestamp. - AgentState lookup_ego_state(const float current_time) const; + void updateAgentHistory( + const double current_time, const TrackedObjects::ConstSharedPtr objects_msg, + const TrackedObject & ego_msg); // Extract target agents and return corresponding indices. // NOTE: Extract targets in order of proximity, closest first. - std::vector extract_target_agents(const std::vector & histories); + std::vector extractTargetAgent(const std::vector & histories); // Return the timestamps relative from the first element.Return the timestamps relative from the // first element. - std::vector get_relative_timestamps() const; + std::vector getRelativeTimestamps() const; // Generate `PredictedObject` from `PredictedTrajectory`. - PredictedObject to_predicted_object( + PredictedObject createPredictedObject( const TrackedObject & object, const PredictedTrajectory & trajectory); // ROS Publisher and Subscriber // TODO(ktro2828): add debug publisher rclcpp::Publisher::SharedPtr pub_objects_; rclcpp::Subscription::SharedPtr sub_objects_; - rclcpp::Subscription::SharedPtr sub_map_; - rclcpp::Subscription::SharedPtr sub_ego_; + rclcpp::Subscription::SharedPtr sub_map_; + // polling subscriber + autoware::universe_utils::InterProcessPollingSubscriber sub_ego_{ + this, "/localization/kinematic_state"}; // Lanelet map pointers std::shared_ptr lanelet_map_ptr_; std::shared_ptr routing_graph_ptr_; std::shared_ptr traffic_rules_ptr_; + std::unique_ptr lanelet_converter_ptr_; // Agent history std::map agent_history_map_; std::map object_msg_map_; + VehicleInfo vehicle_info_; // Pose transform listener autoware::universe_utils::TransformListener transform_listener_; @@ -165,10 +134,9 @@ class MTRNode : public rclcpp::Node std::unique_ptr config_ptr_; std::unique_ptr build_config_ptr_; std::unique_ptr model_ptr_; - PolylineTypeMap polyline_type_map_; - std::shared_ptr polyline_ptr_; - std::vector> ego_states_; - std::vector timestamps_; + + std::unique_ptr>> ego_states_; + std::unique_ptr> timestamps_; }; // class MTRNode } // namespace autoware::mtr #endif // AUTOWARE__MTR__NODE_HPP_ diff --git a/perception/autoware_mtr/include/autoware/mtr/polyline.hpp b/perception/autoware_mtr/include/autoware/mtr/polyline.hpp index 84bd86576f8e9..3b729cf617522 100644 --- a/perception/autoware_mtr/include/autoware/mtr/polyline.hpp +++ b/perception/autoware_mtr/include/autoware/mtr/polyline.hpp @@ -15,9 +15,6 @@ #ifndef AUTOWARE__MTR__POLYLINE_HPP_ #define AUTOWARE__MTR__POLYLINE_HPP_ -#include -#include - #include #include #include @@ -234,72 +231,5 @@ struct PolylineData std::vector data_; const float distance_threshold_; }; - -/** - * @brief Generate LanePoints from LineString. - * - * @param linestring - * @param type_id - * @return std::vector - */ -std::vector getLanePointFromLineString( - const lanelet::ConstLineString3d & linestring, const int type_id) -{ - if (linestring.size() == 0) { - return {}; - } - - const float type_value = static_cast(type_id); - const auto & start = linestring.begin(); - std::vector points{ - {static_cast(start->x()), static_cast(start->y()), static_cast(start->z()), - 0.0f, 0.0f, 0.0f, type_value}}; - points.reserve(linestring.size()); - for (auto itr = start + 1; itr != linestring.end(); ++itr) { - const auto dx = (itr)->x() - (itr - 1)->x(); - const auto dy = (itr)->y() - (itr - 1)->y(); - const auto dz = (itr)->z() - (itr - 1)->z(); - const auto norm = std::hypot(dx, dy, dz); - points.emplace_back( - static_cast(itr->x()), static_cast(itr->y()), static_cast(itr->z()), - static_cast(dx / norm), static_cast(dy / norm), static_cast(dz / norm), - type_value); - } - return points; -} - -/** - * @brief Generate LanePoints from Polygon. - * - * @param polygon - * @param type_id - * @return std::vector - */ -std::vector getLanePointFromPolygon( - const lanelet::CompoundPolygon3d & polygon, const int type_id) -{ - if (polygon.size() == 0) { - return {}; - } - - const float type_value = static_cast(type_id); - const auto & start = polygon.begin(); - std::vector points{ - {static_cast(start->x()), static_cast(start->y()), static_cast(start->z()), - 0.0f, 0.0f, 0.0f, type_value}}; - points.reserve(polygon.size()); - for (auto itr = start + 1; itr != polygon.end(); ++itr) { - const auto dx = (itr)->x() - (itr - 1)->x(); - const auto dy = (itr)->y() - (itr - 1)->y(); - const auto dz = (itr)->z() - (itr - 1)->z(); - const auto norm = std::hypot(dx, dy, dz); - points.emplace_back( - static_cast(itr->x()), static_cast(itr->y()), static_cast(itr->z()), - static_cast(dx / norm), static_cast(dy / norm), static_cast(dz / norm), - type_value); - } - return points; -} - } // namespace autoware::mtr #endif // AUTOWARE__MTR__POLYLINE_HPP_ diff --git a/perception/autoware_mtr/include/autoware/mtr/trajectory.hpp b/perception/autoware_mtr/include/autoware/mtr/trajectory.hpp index f9a18d203187c..24641d8d4999d 100644 --- a/perception/autoware_mtr/include/autoware/mtr/trajectory.hpp +++ b/perception/autoware_mtr/include/autoware/mtr/trajectory.hpp @@ -26,25 +26,26 @@ namespace autoware::mtr constexpr size_t PredictedStateDim = 7; /** - * @brief A class to represent a predicted state. + * @brief A class to represent a predicted state. Note that output elements are (x, y, stdX, + * stdY, rho, vx, vy). */ struct PredictedState { - explicit PredictedState(const std::array & state) + explicit PredictedState(const std::array & state) : x_(state.at(0)), y_(state.at(1)), - dx_(state.at(2)), - dy_(state.at(3)), - yaw_(state.at(4)), + std_x_(state.at(2)), + std_y_(state.at(3)), + rho_(state.at(4)), vx_(state.at(5)), vy_(state.at(6)) { } PredictedState( - const float x, const float y, const float dx, const float dy, const float yaw, const float vx, - const float vy) - : x_(x), y_(y), dx_(dx), dy_(dy), yaw_(yaw), vx_(vx), vy_(vy) + const double x, const double y, const double std_x, const double std_y, const double rho, + const double vx, const double vy) + : x_(x), y_(y), std_x_(std_x), std_y_(std_y), rho_(rho), vx_(vx), vy_(vy) { } @@ -52,28 +53,28 @@ struct PredictedState static size_t dim() { return PredictedStateDim; } // Return the predicted x position. - float x() const { return x_; } + double x() const { return x_; } // Return the predicted y position. - float y() const { return y_; } + double y() const { return y_; } - // Return the predicted dx. - float dx() const { return dx_; } + // Return the predicted std x. + double std_x() const { return std_x_; } - // Return the predicted dy. - float dy() const { return dy_; } + // Return the predicted mean y. + double std_y() const { return std_y_; } - // Return the predicted yaw. - float yaw() const { return yaw_; } + // Return the predicted rho. + double rho() const { return rho_; } // Return the predicted x velocity. - float vx() const { return vx_; } + double vx() const { return vx_; } // Return the predicted y velocity. - float vy() const { return vy_; } + double vy() const { return vy_; } private: - float x_, y_, dx_, dy_, yaw_, vx_, vy_; + double x_, y_, std_x_, std_y_, rho_, vx_, vy_; }; // struct PredictedState /** @@ -81,12 +82,12 @@ struct PredictedState */ struct PredictedMode { - PredictedMode(const float score, const std::vector & waypoints, const size_t num_future) + PredictedMode(const double score, const std::vector & waypoints, const size_t num_future) : score_(score), num_future_(num_future) { for (size_t t = 0; t < num_future_; ++t) { const auto start_itr = waypoints.cbegin() + t * state_dim(); - std::array state; + std::array state; std::copy_n(start_itr, PredictedStateDim, state.begin()); waypoints_.emplace_back(state); } @@ -99,13 +100,13 @@ struct PredictedMode static size_t state_dim() { return PredictedStateDim; } // Return the predicted score. - float score() const { return score_; } + double score() const { return score_; } // Return the vector of waypoints. const std::vector & get_waypoints() const { return waypoints_; } private: - float score_; + double score_; size_t num_future_; std::vector waypoints_; }; // struct PredictedMode @@ -126,14 +127,14 @@ struct PredictedTrajectory * @param num_future The number of predicted timestamps. */ PredictedTrajectory( - const std::vector & scores, const std::vector & modes, const size_t num_mode, + const std::vector & scores, const std::vector & modes, const size_t num_mode, const size_t num_future) : num_mode_(num_mode), num_future_(num_future) { for (size_t m = 0; m < num_mode_; ++m) { const auto score = scores.at(m); const auto wp_itr = modes.cbegin() + m * num_future_ * state_dim(); - std::vector waypoints(wp_itr, wp_itr + num_future_ * state_dim()); + std::vector waypoints(wp_itr, wp_itr + num_future_ * state_dim()); modes_.emplace_back(score, waypoints, num_future_); } // sort by score diff --git a/perception/autoware_mtr/include/autoware/mtr/trt_mtr.hpp b/perception/autoware_mtr/include/autoware/mtr/trt_mtr.hpp index ef5184d78ea09..a22bf806fe152 100644 --- a/perception/autoware_mtr/include/autoware/mtr/trt_mtr.hpp +++ b/perception/autoware_mtr/include/autoware/mtr/trt_mtr.hpp @@ -29,6 +29,7 @@ #include #include #include +#include #include namespace autoware::mtr @@ -50,12 +51,12 @@ struct MTRConfig * @param intention_point_filepath The path to intention points file. * @param num_intention_point_cluster The number of clusters for intension points. */ - MTRConfig( + explicit MTRConfig( const std::vector & target_labels = {"VEHICLE", "PEDESTRIAN", "CYCLIST"}, const size_t num_past = 11, const size_t num_mode = 6, const size_t num_future = 80, const size_t max_num_polyline = 768, const size_t max_num_point = 20, const float point_break_distance = 1.0f, - const std::string & intention_point_filepath = "./data/intention_point.csv", + std::string intention_point_filepath = "./data/intention_point.csv", const size_t num_intention_point_cluster = 64) : target_labels(target_labels), num_class(target_labels.size()), @@ -65,7 +66,7 @@ struct MTRConfig max_num_polyline(max_num_polyline), max_num_point(max_num_point), point_break_distance(point_break_distance), - intention_point_filepath(intention_point_filepath), + intention_point_filepath(std::move(intention_point_filepath)), num_intention_point_cluster(num_intention_point_cluster) { } @@ -97,7 +98,7 @@ class TrtMTR * @param max_workspace_size The max size of workspace. * @param build_config The configuration of build. */ - TrtMTR( + explicit TrtMTR( const std::string & model_path, const MTRConfig & config = MTRConfig(), const BuildConfig & build_config = BuildConfig(), const size_t max_workspace_size = (1ULL << 30)); diff --git a/perception/autoware_mtr/launch/mtr.launch.xml b/perception/autoware_mtr/launch/mtr.launch.xml index db9428c4fb852..11fca98298923 100644 --- a/perception/autoware_mtr/launch/mtr.launch.xml +++ b/perception/autoware_mtr/launch/mtr.launch.xml @@ -6,7 +6,7 @@ - + diff --git a/perception/autoware_mtr/lib/include/attention/trt_attn_value_computation.hpp b/perception/autoware_mtr/lib/include/attention/trt_attn_value_computation.hpp index 1390b80909bf2..70c958c0575ed 100644 --- a/perception/autoware_mtr/lib/include/attention/trt_attn_value_computation.hpp +++ b/perception/autoware_mtr/lib/include/attention/trt_attn_value_computation.hpp @@ -19,7 +19,7 @@ #include -namespace trt_mtr +namespace autoware::trt_mtr { /** * @brief Attention value computation plugin. @@ -97,5 +97,5 @@ class AttentionValueComputationCreator : public TRTPluginCreatorBase const char * name, const void * serialData, size_t serialLength) TRT_NOEXCEPT override; }; // class AttentionValueComputationCreator REGISTER_TENSORRT_PLUGIN(AttentionValueComputationCreator); -} // namespace trt_mtr +} // namespace autoware::trt_mtr #endif // ATTENTION__TRT_ATTN_VALUE_COMPUTATION_HPP_ diff --git a/perception/autoware_mtr/lib/include/attention/trt_attn_weight_computation.hpp b/perception/autoware_mtr/lib/include/attention/trt_attn_weight_computation.hpp index 558b6340ab1c0..42aaf2ecfeba7 100644 --- a/perception/autoware_mtr/lib/include/attention/trt_attn_weight_computation.hpp +++ b/perception/autoware_mtr/lib/include/attention/trt_attn_weight_computation.hpp @@ -19,7 +19,7 @@ #include -namespace trt_mtr +namespace autoware::trt_mtr { /** * @brief The function to compute attention weight. @@ -97,5 +97,5 @@ class AttentionWeightComputationCreator : public TRTPluginCreatorBase const char * name, const void * serialData, size_t serialLength) TRT_NOEXCEPT override; }; // class AttentionWeightComputationCreator REGISTER_TENSORRT_PLUGIN(AttentionWeightComputationCreator); -} // namespace trt_mtr +} // namespace autoware::trt_mtr #endif // ATTENTION__TRT_ATTN_WEIGHT_COMPUTATION_HPP_ diff --git a/perception/autoware_mtr/lib/include/common/trt_plugin_base.hpp b/perception/autoware_mtr/lib/include/common/trt_plugin_base.hpp index 4bfdd11e3d7ef..685cf075c1e43 100644 --- a/perception/autoware_mtr/lib/include/common/trt_plugin_base.hpp +++ b/perception/autoware_mtr/lib/include/common/trt_plugin_base.hpp @@ -25,7 +25,7 @@ #include #include -namespace trt_mtr +namespace autoware::trt_mtr { #if NV_TENSORRT_MAJOR > 7 #define TRT_NOEXCEPT noexcept @@ -168,6 +168,6 @@ class TRTPluginCreatorBase : public nvinfer1::IPluginCreator std::vector mPluginAttributes; std::string mNamespace; }; -} // namespace trt_mtr +} // namespace autoware::trt_mtr #endif // COMMON__TRT_PLUGIN_BASE_HPP_ diff --git a/perception/autoware_mtr/lib/include/knn/trt_knn_batch.hpp b/perception/autoware_mtr/lib/include/knn/trt_knn_batch.hpp index 62aa03aea064a..28077ee6c7b3d 100644 --- a/perception/autoware_mtr/lib/include/knn/trt_knn_batch.hpp +++ b/perception/autoware_mtr/lib/include/knn/trt_knn_batch.hpp @@ -19,7 +19,7 @@ #include -namespace trt_mtr +namespace autoware::trt_mtr { /** * @brief The function to compute KNN batch. @@ -99,5 +99,5 @@ class KnnBatchCreator : public TRTPluginCreatorBase const char * name, const void * serialData, size_t serialLength) TRT_NOEXCEPT override; }; // class KnnBatchCreator REGISTER_TENSORRT_PLUGIN(KnnBatchCreator); -} // namespace trt_mtr +} // namespace autoware::trt_mtr #endif // KNN__TRT_KNN_BATCH_HPP_ diff --git a/perception/autoware_mtr/lib/include/knn/trt_knn_batch_mlogk.hpp b/perception/autoware_mtr/lib/include/knn/trt_knn_batch_mlogk.hpp index 7fb3c54ad802f..71725d79764be 100644 --- a/perception/autoware_mtr/lib/include/knn/trt_knn_batch_mlogk.hpp +++ b/perception/autoware_mtr/lib/include/knn/trt_knn_batch_mlogk.hpp @@ -19,7 +19,7 @@ #include -namespace trt_mtr +namespace autoware::trt_mtr { /** * @brief The function to compute KNN batch with MLogK. @@ -99,5 +99,5 @@ class KnnBatchMlogKCreator : public TRTPluginCreatorBase const char * name, const void * serialData, size_t serialLength) TRT_NOEXCEPT override; }; // class KnnBatchMlogKCreator REGISTER_TENSORRT_PLUGIN(KnnBatchMlogKCreator); -} // namespace trt_mtr +} // namespace autoware::trt_mtr #endif // KNN__TRT_KNN_BATCH_MLOGK_HPP_ diff --git a/perception/autoware_mtr/lib/src/attention/trt_attn_value_computation.cpp b/perception/autoware_mtr/lib/src/attention/trt_attn_value_computation.cpp index 2e9e0a987ab00..ff8b81e0854d6 100644 --- a/perception/autoware_mtr/lib/src/attention/trt_attn_value_computation.cpp +++ b/perception/autoware_mtr/lib/src/attention/trt_attn_value_computation.cpp @@ -16,7 +16,9 @@ #include "attention/trt_attn_value_computation_kernel.hpp" -namespace trt_mtr +#include + +namespace autoware::trt_mtr { namespace { @@ -202,4 +204,4 @@ nvinfer1::IPluginV2DynamicExt * AttentionValueComputationCreator::deserializePlu return plugin; } -} // namespace trt_mtr +} // namespace autoware::trt_mtr diff --git a/perception/autoware_mtr/lib/src/attention/trt_attn_weight_computation.cpp b/perception/autoware_mtr/lib/src/attention/trt_attn_weight_computation.cpp index 1117e425b15b7..39641f021c1f1 100644 --- a/perception/autoware_mtr/lib/src/attention/trt_attn_weight_computation.cpp +++ b/perception/autoware_mtr/lib/src/attention/trt_attn_weight_computation.cpp @@ -16,7 +16,9 @@ #include "attention/trt_attn_weight_computation_kernel.hpp" -namespace trt_mtr +#include + +namespace autoware::trt_mtr { namespace { @@ -204,4 +206,4 @@ nvinfer1::IPluginV2DynamicExt * AttentionWeightComputationCreator::deserializePl return plugin; } -} // namespace trt_mtr +} // namespace autoware::trt_mtr diff --git a/perception/autoware_mtr/lib/src/knn/trt_knn_batch.cpp b/perception/autoware_mtr/lib/src/knn/trt_knn_batch.cpp index 258a8e6428114..86f6438ba9aa1 100644 --- a/perception/autoware_mtr/lib/src/knn/trt_knn_batch.cpp +++ b/perception/autoware_mtr/lib/src/knn/trt_knn_batch.cpp @@ -17,9 +17,9 @@ #include "common/trt_serialize.hpp" #include "knn/trt_knn_batch_kernel.hpp" -#include +#include -namespace trt_mtr +namespace autoware::trt_mtr { namespace { @@ -205,4 +205,4 @@ nvinfer1::IPluginV2DynamicExt * KnnBatchCreator::deserializePlugin( return plugin; } -} // namespace trt_mtr +} // namespace autoware::trt_mtr diff --git a/perception/autoware_mtr/lib/src/knn/trt_knn_batch_mlogk.cpp b/perception/autoware_mtr/lib/src/knn/trt_knn_batch_mlogk.cpp index c438daa59f7e9..618772b03262d 100644 --- a/perception/autoware_mtr/lib/src/knn/trt_knn_batch_mlogk.cpp +++ b/perception/autoware_mtr/lib/src/knn/trt_knn_batch_mlogk.cpp @@ -17,9 +17,9 @@ #include "common/trt_serialize.hpp" #include "knn/trt_knn_batch_mlogk_kernel.hpp" -#include +#include -namespace trt_mtr +namespace autoware::trt_mtr { namespace { @@ -205,4 +205,4 @@ nvinfer1::IPluginV2DynamicExt * KnnBatchMlogKCreator::deserializePlugin( plugin->setPluginNamespace(getPluginNamespace()); return plugin; } -} // namespace trt_mtr +} // namespace autoware::trt_mtr diff --git a/perception/autoware_mtr/lib/src/postprocess/postprocess_kernel.cu b/perception/autoware_mtr/lib/src/postprocess/postprocess_kernel.cu index 381ef00343764..8a6a8ddc96d19 100644 --- a/perception/autoware_mtr/lib/src/postprocess/postprocess_kernel.cu +++ b/perception/autoware_mtr/lib/src/postprocess/postprocess_kernel.cu @@ -35,8 +35,8 @@ __global__ void transformTrajectoryKernel( const float targetCos = cosf(targetYaw); const float targetSin = sinf(targetYaw); - predTrajectory[predIdx] = targetCos * predX + targetSin * predY + targetX; - predTrajectory[predIdx + 1] = -targetSin * predX + targetCos * predY + targetY; + predTrajectory[predIdx] = targetCos * predX - targetSin * predY + targetX; + predTrajectory[predIdx + 1] = targetSin * predX + targetCos * predY + targetY; } cudaError_t postprocessLauncher( diff --git a/perception/autoware_mtr/lib/src/preprocess/polyline_preprocess_kernel.cu b/perception/autoware_mtr/lib/src/preprocess/polyline_preprocess_kernel.cu index f04985d394118..3a826105acce1 100644 --- a/perception/autoware_mtr/lib/src/preprocess/polyline_preprocess_kernel.cu +++ b/perception/autoware_mtr/lib/src/preprocess/polyline_preprocess_kernel.cu @@ -14,6 +14,8 @@ #include "preprocess/polyline_preprocess_kernel.cuh" +#include + #include #include @@ -126,51 +128,54 @@ __global__ void calculateCenterDistanceKernel( distance[b * K + k] = hypot(centerX, centerY); } +template __global__ void extractTopKPolylineKernel( const int K, const int B, const int L, const int P, const int D, const float * inPolyline, const bool * inPolylineMask, const float * inDistance, float * outPolyline, bool * outPolylineMask) { int b = blockIdx.x; // Batch index - int tid = threadIdx.x; // Polyline index + int tid = threadIdx.x; // Thread local index in this CUDA block int p = blockIdx.y * blockDim.y + threadIdx.y; // Point index int d = blockIdx.z * blockDim.z + threadIdx.z; // Dim index - if (b >= B || tid >= L || p >= P || d >= D) { - return; + // Since all threads in a block expected to work wiht CUB, `return` shold not be called here + // if (b >= B || tid >= L || p >= P || d >= D) { + // return; + // } + + // Specialize BlockRadixSort type + using BlockRadixSortT = cub::BlockRadixSort; + using TempStorageT = typename BlockRadixSortT::TempStorage; + + __shared__ TempStorageT temp_storage; + + float distances[ITEMS_PER_THREAD] = {0}; + unsigned int distance_indices[ITEMS_PER_THREAD] = {0}; + for (unsigned int i = 0; i < ITEMS_PER_THREAD; i++) { + int polyline_idx = BLOCK_THREADS * i + tid; // index order don't need to care. + int distance_idx = b * L + polyline_idx; + distance_indices[i] = polyline_idx; + distances[i] = (polyline_idx < L && distance_idx < B * L) ? inDistance[distance_idx] : FLT_MAX; } - extern __shared__ float distances[]; - // Load distances into shared memory - if (tid < L) { - distances[tid] = inDistance[b * L + tid]; - } + BlockRadixSortT(temp_storage).Sort(distances, distance_indices); + // Block-wide sync barrier necessary to refer the sort result __syncthreads(); - // Simple selection of the smallest K distances - // (this part should be replaced with a more efficient sorting/selecting algorithm) - for (int k = 0; k < K; k++) { - float minDistance = FLT_MAX; - int minIndex = -1; - - for (int l = 0; l < L; l++) { - if (distances[l] < minDistance) { - minDistance = distances[l]; - minIndex = l; - } - } - __syncthreads(); + if (b >= B || tid >= L || p >= P || d >= D) { + return; + } - if (minIndex == -1) { + for (unsigned int i = 0; i < ITEMS_PER_THREAD; i++) { + int consective_polyline_idx = + tid * ITEMS_PER_THREAD + i; // To keep sorted order, theads have to write consective region + int inIdx = b * L * P + distance_indices[i] * P + p; + int outIdx = b * K * P + consective_polyline_idx * P + p; + if (consective_polyline_idx >= K || fabsf(FLT_MAX - distances[i]) < FLT_EPSILON) { continue; } - - if (tid == k) { // this thread will handle copying the k-th smallest polyline - int inIdx = b * L * P + minIndex * P + p; - int outIdx = b * K * P + k * P + p; - outPolyline[outIdx * D + d] = inPolyline[inIdx * D + d]; - outPolylineMask[outIdx] = inPolylineMask[inIdx]; - } - distances[minIndex] = FLT_MAX; // exclude this index from future consideration + outPolyline[outIdx * D + d] = inPolyline[inIdx * D + d]; + outPolylineMask[outIdx] = inPolylineMask[inIdx]; } } @@ -192,7 +197,7 @@ __global__ void calculatePolylineCenterKernel( } // calculate polyline center - float sumX = 0.0f, sumY = 0.0f, sumZ = 0.0f; + double sumX = 0.0, sumY = 0.0, sumZ = 0.0; int numValid = 0; for (int p = 0; p < P; ++p) { int idx = b * K * P + k * P + p; @@ -204,9 +209,9 @@ __global__ void calculatePolylineCenterKernel( } } - center[centerIdx] = sumX / fmaxf(1.0f, numValid); - center[centerIdx + 1] = sumY / fmaxf(1.0f, numValid); - center[centerIdx + 2] = sumZ / fmaxf(1.0f, numValid); + center[centerIdx] = static_cast(sumX / fmaxf(1.0f, numValid)); + center[centerIdx + 1] = static_cast(sumY / fmaxf(1.0f, numValid)); + center[centerIdx + 2] = static_cast(sumZ / fmaxf(1.0f, numValid)); } cudaError_t polylinePreprocessWithTopkLauncher( @@ -233,10 +238,16 @@ cudaError_t polylinePreprocessWithTopkLauncher( B, L, P, outPointDim, tmpPolyline, tmpPolylineMask, tmpDistance); const dim3 blocks3(B, P, outPointDim); - const size_t sharedMemSize = sizeof(float) * L; - extractTopKPolylineKernel<<>>( - K, B, L, P, outPointDim, tmpPolyline, tmpPolylineMask, tmpDistance, outPolyline, - outPolylineMask); + constexpr unsigned int itemsPerThread = 24; + if (threadsPerBlock * itemsPerThread < L) { + std::cerr << "Larger L (" << L << ") than acceptable range (< " + << threadsPerBlock * itemsPerThread << ") detected." << std::endl; + return cudaError_t::cudaErrorInvalidValue; + } + extractTopKPolylineKernel + <<>>( + K, B, L, P, outPointDim, tmpPolyline, tmpPolylineMask, tmpDistance, outPolyline, + outPolylineMask); const dim3 blocks4(B, K, P); setPreviousPositionKernel<<>>( diff --git a/perception/autoware_mtr/package.xml b/perception/autoware_mtr/package.xml index 0ab45469f4ff3..bcdb22fab2713 100644 --- a/perception/autoware_mtr/package.xml +++ b/perception/autoware_mtr/package.xml @@ -10,10 +10,12 @@ ament_cmake_auto autoware_cmake - autoware_auto_mapping_msgs - autoware_auto_perception_msgs + autoware_map_msgs autoware_object_recognition_utils + autoware_perception_msgs autoware_tensorrt_common + autoware_universe_utils + autoware_vehicle_info_utils lanelet2_core lanelet2_extension lanelet2_routing @@ -21,7 +23,6 @@ nav_msgs rclcpp rclcpp_components - tier4_autoware_utils ament_lint_auto autoware_lint_common diff --git a/perception/autoware_mtr/src/conversions/history.cpp b/perception/autoware_mtr/src/conversions/history.cpp new file mode 100644 index 0000000000000..8175af8ba493e --- /dev/null +++ b/perception/autoware_mtr/src/conversions/history.cpp @@ -0,0 +1,111 @@ +// Copyright 2024 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 "autoware/mtr/conversions/history.hpp" + +#include +#include + +namespace autoware::mtr +{ +using adl = autoware::mtr::AgentDimLabels; + +/** + * @brief Transform a 2D point with a reference point and rotation + * + * @param history history to modify. + * @param reference_state reference state used as reference frame. + * @return vector of histories, each in the reference frame of the last state. + */ +[[nodiscard]] std::pair transform2D( + const std::pair input_xy, const std::pair reference_xy, + const std::pair rotation_cos_sin) +{ + auto [x, y] = input_xy; + auto [ref_x, ref_y] = reference_xy; + auto [cos, sin] = rotation_cos_sin; + x -= ref_x; + y -= ref_y; + return {x * cos - y * sin, x * sin + y * cos}; +} + +/** + * @brief Get histories with agent states in the reference frame of the last state + * + * @param history history to modify. + * @param reference_state reference state used as reference frame. + * @return vector of histories, each in the reference frame of the last state. + */ +[[nodiscard]] AgentHistory getAgentCentricHistory( + const AgentHistory & history, const AgentState & reference_state) +{ + const auto latest_valid_state = history.get_latest_valid_state(); + const auto & reference_state_dim = autoware::mtr::AgentState::dim(); + const auto & history_state_dim = autoware::mtr::AgentHistory::state_dim(); + + if (!latest_valid_state.has_value() || reference_state_dim != history_state_dim) { + return history; + } + AgentHistory agent_centric_history = history; + const auto reference_array = reference_state.as_array(); + auto agent_centric_states_array = agent_centric_history.as_array(); + const auto ref_x = reference_array.at(adl::X); + const auto ref_y = reference_array.at(adl::Y); + const auto ref_z = reference_array.at(adl::Z); + const auto ref_yaw = reference_array.at(adl::YAW); + const auto cos = std::cos(-ref_yaw); + const auto sin = std::sin(-ref_yaw); + + const auto & d = reference_state_dim; + // Use i as the index for each new state + for (size_t i = 0; i < agent_centric_states_array.size(); i += d) { + auto & x = agent_centric_states_array.at(i + adl::X); + auto & y = agent_centric_states_array.at(i + adl::Y); + auto & vx = agent_centric_states_array.at(i + adl::VX); + auto & vy = agent_centric_states_array.at(i + adl::VY); + auto & ax = agent_centric_states_array.at(i + adl::AX); + auto & ay = agent_centric_states_array.at(i + adl::AY); + auto & z = agent_centric_states_array.at(i + adl::Z); + auto & yaw = agent_centric_states_array.at(i + adl::YAW); + + std::tie(x, y) = transform2D({x, y}, {ref_x, ref_y}, {cos, sin}); + std::tie(vx, vy) = transform2D({vx, vy}, {0.0, 0.0}, {cos, sin}); + std::tie(ax, ay) = transform2D({x, y}, {0.0, 0.0}, {cos, sin}); + + z -= ref_z; + yaw -= ref_yaw; + } + return agent_centric_history; +}; + +/** + * @brief Get histories with agent states in the reference frame of the last state + * + * @param histories vector of histories to modify. + * @return vector of histories, each in the reference frame of the last state. + */ +[[nodiscard]] std::vector getAgentCentricHistories( + const std::vector & histories) +{ + std::vector agent_centric_histories; + agent_centric_histories.reserve(histories.size()); + for (const auto & history : histories) { + const auto & reference_state = + history.get_latest_state(); // Todo(Daniel): should it be the latest VALID state? + agent_centric_histories.push_back(getAgentCentricHistory(history, reference_state)); + } + return agent_centric_histories; +}; + +} // namespace autoware::mtr diff --git a/perception/autoware_mtr/src/conversions/lanelet.cpp b/perception/autoware_mtr/src/conversions/lanelet.cpp new file mode 100644 index 0000000000000..3512c4283f83b --- /dev/null +++ b/perception/autoware_mtr/src/conversions/lanelet.cpp @@ -0,0 +1,142 @@ +// Copyright 2024 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 "autoware/mtr/conversions/lanelet.hpp" + +#include "autoware/mtr/polyline.hpp" + +#include + +#include +#include +#include + +namespace autoware::mtr +{ +std::optional LaneletConverter::convert( + const geometry_msgs::msg::Point & position, double distance_threshold) const +{ + std::vector container; + // parse lanelet layers + for (const auto & lanelet : lanelet_map_ptr_->laneletLayer) { + const auto lanelet_subtype = toSubtypeName(lanelet); + if (isLaneLike(lanelet_subtype)) { + // convert centerlines + if (isRoadwayLike(lanelet_subtype)) { + auto points = fromLinestring(lanelet.centerline3d(), position, distance_threshold); + insertLanePoints(points, container); + } + // convert boundaries except of virtual lines + if (!isTurnableIntersection(lanelet)) { + const auto left_bound = lanelet.leftBound3d(); + if (isBoundaryLike(left_bound)) { + auto points = fromLinestring(left_bound, position, distance_threshold); + insertLanePoints(points, container); + } + const auto right_bound = lanelet.rightBound3d(); + if (isBoundaryLike(right_bound)) { + auto points = fromLinestring(right_bound, position, distance_threshold); + insertLanePoints(points, container); + } + } + } else if (isCrosswalkLike(lanelet_subtype)) { + auto points = fromPolygon(lanelet.polygon3d(), position, distance_threshold); + insertLanePoints(points, container); + } + } + + // parse linestring layers + for (const auto & linestring : lanelet_map_ptr_->lineStringLayer) { + if (isBoundaryLike(linestring)) { + auto points = fromLinestring(linestring, position, distance_threshold); + insertLanePoints(points, container); + } + } + + return container.size() == 0 + ? std::nullopt + : std::make_optional( + container, max_num_polyline_, max_num_point_, point_break_distance_); +} + +std::vector LaneletConverter::fromLinestring( + const lanelet::ConstLineString3d & linestring, const geometry_msgs::msg::Point & position, + double distance_threshold) const noexcept +{ + if (linestring.size() == 0) { + return {}; + } + + std::vector output; + for (auto itr = linestring.begin(); itr != linestring.end(); ++itr) { + if (auto distance = + std::hypot(itr->x() - position.x, itr->y() - position.y, itr->z() - position.z); + distance > distance_threshold) { + continue; + } + double dx, dy, dz; + constexpr double epsilon = 1e-6; + if (itr == linestring.begin()) { + dx = 0.0; + dy = 0.0; + dz = 0.0; + } else { + dx = itr->x() - (itr - 1)->x(); + dy = itr->y() - (itr - 1)->y(); + dz = itr->z() - (itr - 1)->z(); + const auto norm = std::hypot(dx, dy, dz); + dx /= (norm + epsilon); + dy /= (norm + epsilon); + dz /= (norm + epsilon); + } + output.emplace_back(itr->x(), itr->y(), itr->z(), dx, dy, dz, 0.0); // TODO(ktro2828): Label ID + } + return output; +} + +std::vector LaneletConverter::fromPolygon( + const lanelet::CompoundPolygon3d & polygon, const geometry_msgs::msg::Point & position, + double distance_threshold) const noexcept +{ + if (polygon.size() == 0) { + return {}; + } + + std::vector output; + for (auto itr = polygon.begin(); itr != polygon.end(); ++itr) { + if (auto distance = + std::hypot(itr->x() - position.x, itr->y() - position.y, itr->z() - position.z); + distance > distance_threshold) { + continue; + } + double dx, dy, dz; + constexpr double epsilon = 1e-6; + if (itr == polygon.begin()) { + dx = 0.0; + dy = 0.0; + dz = 0.0; + } else { + dx = itr->x() - (itr - 1)->x(); + dy = itr->y() - (itr - 1)->y(); + dz = itr->z() - (itr - 1)->z(); + const auto norm = std::hypot(dx, dy, dz); + dx /= (norm + epsilon); + dy /= (norm + epsilon); + dz /= (norm + epsilon); + } + output.emplace_back(itr->x(), itr->y(), itr->z(), dx, dy, dz, 0.0); // TODO(ktro2828): Label ID + } + return output; +} +} // namespace autoware::mtr diff --git a/perception/autoware_mtr/src/intention_point.cpp b/perception/autoware_mtr/src/intention_point.cpp new file mode 100644 index 0000000000000..9b4036c99e143 --- /dev/null +++ b/perception/autoware_mtr/src/intention_point.cpp @@ -0,0 +1,41 @@ +// Copyright 2024 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 "autoware/mtr/intention_point.hpp" + +#include +#include +#include +#include + +namespace autoware::mtr +{ +std::vector IntentionPoint::as_array(const std::vector & label_names) const +{ + std::vector points; + points.reserve(label_names.size() * num_cluster * state_dim()); + for (const auto & name : label_names) { + const auto & label_points = data_map_.at(name); + for (const auto & p : label_points) { + points.emplace_back(p); + } + } + return points; +} + +size_t IntentionPoint::size() const noexcept +{ + return num_cluster * state_dim(); +} +} // namespace autoware::mtr diff --git a/perception/autoware_mtr/src/node.cpp b/perception/autoware_mtr/src/node.cpp index 7dae029e40f31..57ea7042e2d2c 100644 --- a/perception/autoware_mtr/src/node.cpp +++ b/perception/autoware_mtr/src/node.cpp @@ -14,153 +14,75 @@ #include "autoware/mtr/node.hpp" -#include +#include "autoware/mtr/agent.hpp" +#include "autoware/mtr/conversions/history.hpp" +#include "autoware/mtr/conversions/lanelet.hpp" +#include "autoware/mtr/fixed_queue.hpp" + #include +#include +#include +#include #include #include +#include #include #include #include +#include +#include +#include +#include namespace autoware::mtr { +// TODO(ktro2828): use a parameter +constexpr double TIME_THRESHOLD = 1.0; +constexpr size_t MAX_NUM_TARGET = 1; +constexpr double POLYLINE_DISTANCE_THRESHOLD = 100.0; namespace { -// Return the Lanelet subtype name. If input Lanelet has no attribute named `type` return empty -// string `""`. -std::string subtype_from(const lanelet::Lanelet & lanelet) -{ - if (!lanelet.hasAttribute("subtype")) { - return ""; - } else { - const auto subtype = lanelet.attribute("subtype").as(); - return subtype ? subtype.get() : ""; - } -} - -// Return the LineString type name. If input LineString has no attribute named `type` return empty -// string `""`. -std::string linestring_type_from(const lanelet::ConstLineString3d & linestring) -{ - if (!linestring.hasAttribute("type")) { - return ""; - } else { - const auto type = linestring.attribute("type").as(); - return type ? type.get() : ""; - } -} - -// Return the LineString subtype name. If input LineString has no attribute named `subtype` return -// empty string `""`. -std::string linestring_subtype_from(const lanelet::ConstLineString3d & linestring) -{ - if (!linestring.hasAttribute("subtype")) { - return ""; - } else { - const auto subtype = linestring.attribute("subtype").as(); - return subtype ? subtype.get() : ""; - } -} - -// Check whether lanelet has an attribute named `turn_direction`. -bool is_turn_intersection(const lanelet::Lanelet & lanelet) -{ - if (lanelet.hasAttribute("turn_direction")) { - return true; - } else { - return false; - } -} - -// Insert source `LanePoints` into target `LanePoints`. -void insert_lane_points(const std::vector & src, std::vector & dst) -{ - dst.reserve(dst.size() * 2); - dst.insert(dst.end(), src.cbegin(), src.cend()); -} - // Convert `TrackedObject` to `AgentState`. -AgentState to_agent_state(const TrackedObject & object, const bool is_valid) +AgentState createAgentState(const TrackedObject & object, const bool is_valid) { const auto & pose = object.kinematics.pose_with_covariance.pose; const auto & twist = object.kinematics.twist_with_covariance.twist; const auto & accel = object.kinematics.acceleration_with_covariance.accel; - const auto & dimensions = object.shape.dimensions; - const auto yaw = tf2::getYaw(pose.orientation); - const auto valid = is_valid ? 1.0f : 0.0f; - - return { - static_cast(pose.position.x), - static_cast(pose.position.y), - static_cast(pose.position.z), - static_cast(dimensions.x), - static_cast(dimensions.y), - static_cast(dimensions.z), - static_cast(yaw), - static_cast(twist.linear.x), - static_cast(twist.linear.y), - static_cast(accel.linear.x), - static_cast(accel.linear.y), - valid}; + const auto & dimension = object.shape.dimensions; + + const float yaw = tf2::getYaw(pose.orientation); + const float valid = is_valid ? 1.0f : 0.0f; + + return {pose.position, dimension, yaw, twist.linear, accel.linear, valid}; } // Get the label index corresponding to AgentLabel. If the label of tracked object is not * defined // in AgentLabel returns `-1`. -int label_index_from(const TrackedObject & object) +int getLabelIndex(const TrackedObject & object) { const auto classification = autoware::object_recognition_utils::getHighestProbLabel(object.classification); if (autoware::object_recognition_utils::isCarLikeVehicle(classification)) { return AgentLabel::VEHICLE; - } else if (classification == ObjectClassification::PEDESTRIAN) { + } + if (classification == ObjectClassification::PEDESTRIAN) { return AgentLabel::PEDESTRIAN; - } else if ( + } + if ( classification == ObjectClassification::MOTORCYCLE || classification == ObjectClassification::BICYCLE) { return AgentLabel::CYCLIST; - } else { - return -1; // other labels - } -} - -// Return corresponding PrecisionType from string. -PrecisionType as_precision_type(const std::string & name) -{ - if (name == "FP32") { - return PrecisionType::FP32; - } else if (name == "FP16") { - return PrecisionType::FP16; - } else if (name == "INT8") { - return PrecisionType::INT8; - } else { - throw std::invalid_argument("Invalid precision name."); - } -} - -// Return corresponding CalibrationType from string. -CalibrationType as_calibration_type(const std::string & name) -{ - if (name == "ENTROPY") { - return CalibrationType::ENTROPY; - } else if (name == "LEGACY") { - return CalibrationType::LEGACY; - } else if (name == "PERCENTILE") { - return CalibrationType::PERCENTILE; - } else if (name == "MINMAX") { - return CalibrationType::MINMAX; - } else { - throw std::invalid_argument("Invalid calibration name."); } + return -1; // other labels } } // namespace MTRNode::MTRNode(const rclcpp::NodeOptions & node_options) -: rclcpp::Node("mtr", node_options), transform_listener_(this), polyline_type_map_(this) +: rclcpp::Node("mtr", node_options), transform_listener_(this) { - // TODO(ktro2828) { // Build MTR // Model config @@ -185,21 +107,24 @@ MTRNode::MTRNode(const rclcpp::NodeOptions & node_options) point_break_distance, intention_point_filepath, num_intention_point_cluster); // Build config const auto is_dynamic = declare_parameter("build_params.is_dynamic"); - const auto precision_str = declare_parameter("build_params.precision"); - const auto calibration_str = declare_parameter("build_params.calibration"); - const auto precision = as_precision_type(precision_str); - const auto calibration = as_calibration_type(calibration_str); + const auto precision = declare_parameter("build_params.precision"); + const auto calibration = declare_parameter("build_params.calibration"); build_config_ptr_ = std::make_unique(is_dynamic, precision, calibration); model_ptr_ = std::make_unique(model_path, *config_ptr_, *build_config_ptr_); } + { + // Ego states and timestamps buffer + int ego_buffer_size = declare_parameter("ego_buffer_size", 100); + ego_states_ = std::make_unique>>(ego_buffer_size); + timestamps_ = std::make_unique>(config_ptr_->num_past); + } + sub_objects_ = create_subscription( "~/input/objects", rclcpp::QoS{1}, std::bind(&MTRNode::callback, this, std::placeholders::_1)); - sub_map_ = create_subscription( + sub_map_ = create_subscription( "~/input/vector_map", rclcpp::QoS{1}.transient_local(), - std::bind(&MTRNode::on_map, this, std::placeholders::_1)); - sub_ego_ = create_subscription( - "~/input/ego", rclcpp::QoS{1}, std::bind(&MTRNode::on_ego, this, std::placeholders::_1)); + std::bind(&MTRNode::onMap, this, std::placeholders::_1)); pub_objects_ = create_publisher("~/output/objects", rclcpp::QoS{1}); @@ -211,58 +136,62 @@ MTRNode::MTRNode(const rclcpp::NodeOptions & node_options) void MTRNode::callback(const TrackedObjects::ConstSharedPtr object_msg) { - if (!polyline_ptr_) { - RCLCPP_WARN(get_logger(), "No polyline"); + const auto ego_msg = getLatestEgo(); + + if (!ego_msg) { + RCLCPP_WARN(get_logger(), "No ego data"); return; } - const auto current_time = static_cast(rclcpp::Time(object_msg->header.stamp).seconds()); + const auto polyline_data = lanelet_converter_ptr_->convert( + ego_msg->kinematics.pose_with_covariance.pose.position, POLYLINE_DISTANCE_THRESHOLD); - timestamps_.emplace_back(current_time); - // TODO(ktro2828): update timestamps - if (timestamps_.size() < config_ptr_->num_past) { - RCLCPP_WARN(get_logger(), "Not enough timestamp"); - return; // Not enough timestamps - } else if (config_ptr_->num_past < timestamps_.size()) { - timestamps_.erase(timestamps_.begin(), timestamps_.begin()); + if (!polyline_data) { + RCLCPP_WARN(get_logger(), "No polyline"); + return; } - remove_ancient_history(current_time, object_msg); - update_history(current_time, object_msg); + const auto current_time = rclcpp::Time(object_msg->header.stamp).seconds(); + timestamps_->push_back(current_time); + + removeAncientAgentHistory(current_time, object_msg); + updateAgentHistory(current_time, object_msg, ego_msg.value()); std::vector object_ids; std::vector histories; - std::vector label_indices; + std::vector label_ids; histories.reserve(agent_history_map_.size()); object_ids.reserve(agent_history_map_.size()); - label_indices.reserve(agent_history_map_.size()); - int sdc_index = -1; + label_ids.reserve(agent_history_map_.size()); + int tmp_ego_index = -1; for (const auto & [object_id, history] : agent_history_map_) { object_ids.emplace_back(object_id); histories.emplace_back(history); - label_indices.emplace_back(history.label_index()); + label_ids.emplace_back(history.label_id()); if (object_id == EGO_ID) { - sdc_index = histories.size() - 1; + tmp_ego_index = histories.size() - 1; } } - if (sdc_index == -1) { + if (tmp_ego_index == -1) { RCLCPP_WARN(get_logger(), "No EGO"); return; } - const auto target_indices = extract_target_agents(histories); + const auto target_indices = extractTargetAgent(histories); if (target_indices.empty()) { RCLCPP_WARN(get_logger(), "No target agents"); return; } - const auto relative_timestamps = get_relative_timestamps(); - AgentData agent_data( - histories, static_cast(sdc_index), target_indices, label_indices, relative_timestamps); + const auto ego_index = static_cast(tmp_ego_index); + const auto relative_timestamps = getRelativeTimestamps(); + // For testing purposes, normally pre-processing is done in the cuda side. + // const auto agent_centric_histories = getAgentCentricHistories(histories); + AgentData agent_data(histories, ego_index, target_indices, label_ids, relative_timestamps); std::vector trajectories; - if (!model_ptr_->doInference(agent_data, *polyline_ptr_, trajectories)) { + if (!model_ptr_->doInference(agent_data, *polyline_data, trajectories)) { RCLCPP_WARN(get_logger(), "Inference failed"); return; } @@ -271,11 +200,12 @@ void MTRNode::callback(const TrackedObjects::ConstSharedPtr object_msg) output.header = object_msg->header; output.objects.reserve(target_indices.size()); for (size_t i = 0; i < target_indices.size(); ++i) { - const auto & trajectory = trajectories.at(i); const auto & target_idx = target_indices.at(i); const auto & object_id = object_ids.at(target_idx); const auto & object = object_msg_map_.at(object_id); - auto predicted_object = to_predicted_object(object, trajectory); + const auto & trajectory = trajectories.at(i); + + auto predicted_object = createPredictedObject(object, trajectory); output.objects.emplace_back(predicted_object); } @@ -283,174 +213,134 @@ void MTRNode::callback(const TrackedObjects::ConstSharedPtr object_msg) pub_objects_->publish(output); } -void MTRNode::on_map(const LaneletMapBin::ConstSharedPtr map_msg) +void MTRNode::onMap(const HADMapBin::ConstSharedPtr map_msg) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( *map_msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); - RCLCPP_DEBUG(get_logger(), "[TensorRT MTR]: Start converting lanelet to polyline"); - if (lanelet_to_polyline()) { - RCLCPP_DEBUG(get_logger(), "[TensorRT MTR]: Success to convert lanelet to polyline"); - } else { - RCLCPP_WARN(get_logger(), "[TensorRT MTR]: Fail to convert lanelet to polyline"); - } -} - -void MTRNode::on_ego(const Odometry::ConstSharedPtr ego_msg) -{ - const auto current_time = static_cast(rclcpp::Time(ego_msg->header.stamp).seconds()); - const auto & position = ego_msg->pose.pose.position; - const auto & twist = ego_msg->twist.twist; - const auto yaw = static_cast(tf2::getYaw(ego_msg->pose.pose.orientation)); - float ax = 0.0f, ay = 0.0f; - if (!ego_states_.empty()) { - const auto & latest_state = ego_states_.back(); - const auto time_diff = current_time - latest_state.first; - ax = static_cast(twist.linear.x) - latest_state.second.vx() / (time_diff + 1e-10f); - ay = static_cast(twist.linear.y) - latest_state.second.vy() / (time_diff + 1e-10f); - } - - // TODO(ktro2828): use received ego size topic - ego_states_.emplace_back( - current_time, - AgentState( - static_cast(position.x), static_cast(position.y), - static_cast(position.z), EGO_LENGTH, EGO_WIDTH, EGO_HEIGHT, yaw, - static_cast(twist.linear.x), static_cast(twist.linear.y), ax, ay, true)); - - constexpr size_t max_buffer_size = 100; - if (max_buffer_size < ego_states_.size()) { - ego_states_.erase(ego_states_.begin(), ego_states_.begin()); - } + lanelet_converter_ptr_ = std::make_unique( + lanelet_map_ptr_, config_ptr_->max_num_polyline, config_ptr_->max_num_point, + config_ptr_->point_break_distance); } -bool MTRNode::lanelet_to_polyline() +std::optional MTRNode::getLatestEgo() { - if (!lanelet_map_ptr_) { - return false; - } - - std::vector all_points; - for (const auto & lanelet : lanelet_map_ptr_->laneletLayer) { - const auto lanelet_subtype = subtype_from(lanelet); - if ( - lanelet_subtype == "road" || lanelet_subtype == "highway" || - lanelet_subtype == "road_shoulder" || lanelet_subtype == "pedestrian_lane" || - lanelet_subtype == "bicycle_lane" || lanelet_subtype == "walkway") { - if ( - lanelet_subtype == "road" || lanelet_subtype == "highway" || - lanelet_subtype == "road_shoulder") { - const auto & type_id = polyline_type_map_.type_to_id(lanelet_subtype); - auto points = getLanePointFromLineString(lanelet.centerline3d(), type_id); - insert_lane_points(points, all_points); - } - if (!is_turn_intersection(lanelet)) { - const auto & left = lanelet.leftBound3d(); - const auto left_type = linestring_type_from(left); - if (left_type == "line_thin" || left_type == "line_thick") { - const auto left_subtype = linestring_subtype_from(left); - const auto & type_id = polyline_type_map_.type_to_id(left_subtype); - if (type_id != -1) { - auto points = getLanePointFromLineString(left, type_id); - insert_lane_points(points, all_points); - } - } - const auto & right = lanelet.rightBound3d(); - const auto right_type = linestring_type_from(right); - if (right_type == "line_thin" || right_type == "line_thick") { - const auto right_subtype = linestring_subtype_from(right); - const auto & type_id = polyline_type_map_.type_to_id(right_subtype); - if (type_id != -1) { - auto points = getLanePointFromLineString(right, type_id); - insert_lane_points(points, all_points); - } - } - } - } else if (lanelet_subtype == "crosswalk") { - const auto & type_id = polyline_type_map_.type_to_id(lanelet_subtype); - auto points = getLanePointFromPolygon(lanelet.polygon3d(), type_id); - insert_lane_points(points, all_points); - } + const Odometry::ConstSharedPtr odometry_msg = sub_ego_.takeData(); + if (!odometry_msg) { + return std::nullopt; + } + + auto createPoint32 = + [](const double x, const double y, const double z) -> geometry_msgs::msg::Point32 { + geometry_msgs::msg::Point32 p; + p.x = x; + p.y = y; + p.z = z; + return p; + }; + + TrackedObject output; + // Classification and probability + { + output.existence_probability = 1.0; + ObjectClassification classification; + classification.label = ObjectClassification::CAR; + output.classification = {classification}; } - for (const auto & linestring : lanelet_map_ptr_->lineStringLayer) { - const auto linestring_type = linestring_type_from(linestring); - if (linestring_type != "road_boarder" && linestring_type != "traffic_sign") { - continue; - } - const auto & type_id = polyline_type_map_.type_to_id(linestring_type); - auto points = getLanePointFromLineString(linestring, type_id); - insert_lane_points(points, all_points); + // Kinematics + { + output.kinematics.pose_with_covariance = odometry_msg->pose; + output.kinematics.twist_with_covariance = odometry_msg->twist; } - if (all_points.size() == 0) { - return false; - } else { - polyline_ptr_ = std::make_shared( - all_points, config_ptr_->max_num_polyline, config_ptr_->max_num_point, - config_ptr_->point_break_distance); - return true; + // Shape + { + const auto & ego_max_long_offset = vehicle_info_.max_longitudinal_offset_m; + const auto & ego_rear_overhang = vehicle_info_.vehicle_height_m; + const auto & ego_length = vehicle_info_.vehicle_length_m; + const auto & ego_width = vehicle_info_.vehicle_width_m; + const auto & ego_height = vehicle_info_.vehicle_height_m; + + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + shape.dimensions.x = ego_length; + shape.dimensions.y = ego_width; + shape.dimensions.z = ego_height; + + // TODO(Daniel): Should use overhang and ego info utils + shape.footprint.points.emplace_back( + createPoint32(-ego_rear_overhang, -0.5 * ego_width, ego_height)); + shape.footprint.points.emplace_back( + createPoint32(-ego_rear_overhang, 0.5 * ego_width, ego_height)); + shape.footprint.points.emplace_back( + createPoint32(ego_max_long_offset, 0.5 * ego_width, ego_height)); + shape.footprint.points.emplace_back( + createPoint32(ego_max_long_offset, -0.5 * ego_width, ego_height)); + output.shape = shape; } + return output; } -void MTRNode::remove_ancient_history( - const float current_time, const TrackedObjects::ConstSharedPtr objects_msg) +void MTRNode::removeAncientAgentHistory( + const double current_time, const TrackedObjects::ConstSharedPtr objects_msg) { - constexpr float time_threshold = 1.0f; // TODO(ktro2828): use parameter for (const auto & object : objects_msg->objects) { - const auto & object_id = autoware_utils::to_hex_string(object.object_id); + const auto & object_id = autoware::universe_utils::toHexString(object.object_id); if (agent_history_map_.count(object_id) == 0) { continue; } const auto & history = agent_history_map_.at(object_id); - if (history.is_ancient(current_time, time_threshold)) { + if (history.is_ancient(current_time, TIME_THRESHOLD)) { agent_history_map_.erase(object_id); } } if ( agent_history_map_.count(EGO_ID) != 0 && - agent_history_map_.at(EGO_ID).is_ancient(current_time, time_threshold)) { + agent_history_map_.at(EGO_ID).is_ancient(current_time, TIME_THRESHOLD)) { agent_history_map_.erase(EGO_ID); } } -void MTRNode::update_history( - const float current_time, const TrackedObjects::ConstSharedPtr objects_msg) +void MTRNode::updateAgentHistory( + const double current_time, const TrackedObjects::ConstSharedPtr objects_msg, + const TrackedObject & ego_msg) { - // TODO(ktro2828): use ego info std::vector observed_ids; + // other agents for (const auto & object : objects_msg->objects) { - auto label_index = label_index_from(object); + auto label_index = getLabelIndex(object); if (label_index == -1) { continue; } - const auto & object_id = autoware_utils::to_hex_string(object.object_id); + const auto object_id = autoware::universe_utils::toHexString(object.object_id); observed_ids.emplace_back(object_id); - object_msg_map_.emplace(object_id, object); - auto state = to_agent_state(object, true); + object_msg_map_.insert_or_assign(object_id, object); + const auto state = createAgentState(object, true); if (agent_history_map_.count(object_id) == 0) { - AgentHistory history(object_id, label_index, config_ptr_->num_past); - history.update(current_time, state); + AgentHistory history(state, object_id, label_index, current_time, config_ptr_->num_past); agent_history_map_.emplace(object_id, history); } else { agent_history_map_.at(object_id).update(current_time, state); } } - auto ego_state = lookup_ego_state(current_time); + // ego vehicle + observed_ids.emplace_back(EGO_ID); + object_msg_map_.insert_or_assign(EGO_ID, ego_msg); + + const auto ego = createAgentState(ego_msg, true); if (agent_history_map_.count(EGO_ID) == 0) { - AgentHistory history(EGO_ID, AgentLabel::VEHICLE, config_ptr_->num_past); - history.update(current_time, ego_state); + AgentHistory history(ego, EGO_ID, AgentLabel::VEHICLE, current_time, config_ptr_->num_past); agent_history_map_.emplace(EGO_ID, history); } else { - agent_history_map_.at(EGO_ID).update(current_time, ego_state); + agent_history_map_.at(EGO_ID).update(current_time, ego); } - observed_ids.emplace_back(EGO_ID); // update unobserved histories with empty for (auto & [object_id, history] : agent_history_map_) { @@ -461,32 +351,21 @@ void MTRNode::update_history( } } -AgentState MTRNode::lookup_ego_state(const float current_time) const +std::vector MTRNode::extractTargetAgent(const std::vector & histories) { - auto state = std::min_element( - ego_states_.cbegin(), ego_states_.cend(), [&](const auto & s1, const auto & s2) { - return std::abs(s1.first - current_time) < std::abs(s2.first - current_time); - }); - return state->second; -} + auto map2ego = transform_listener_.getTransform( + "base_link", "map", rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)); -std::vector MTRNode::extract_target_agents(const std::vector & histories) -{ - std::vector> distances; - for (size_t i = 0; i < histories.size(); ++i) { - const auto & history = histories.at(i); - if (!history.is_valid_latest() || history.object_id() == EGO_ID) { - distances.emplace_back(i, INFINITY); - } else { - auto map2ego = transform_listener_.getTransform( - "base_link", // target - "map", // src - rclcpp::Time(), rclcpp::Duration::from_seconds(0.1)); - if (!map2ego) { - RCLCPP_WARN(get_logger(), "Failed to get transform from map to base_link."); - return {}; - } - const auto state = history.get_latest_state(); + if (!map2ego) { + RCLCPP_WARN(get_logger(), "Failed to get transform from map to base_link."); + return {}; + } + + std::vector> index_distances; + for (size_t idx = 0; idx < histories.size(); ++idx) { + const auto & history = histories.at(idx); + if (history.is_valid_latest() && history.object_id() != EGO_ID) { + const auto & state = history.get_latest_state(); geometry_msgs::msg::PoseStamped pose_in_map; pose_in_map.pose.position.x = state.x(); pose_in_map.pose.position.y = state.y(); @@ -496,22 +375,20 @@ std::vector MTRNode::extract_target_agents(const std::vector target_indices; - target_indices.reserve(max_target_size); - for (const auto & [idx, _] : distances) { + for (const auto & [idx, _] : index_distances) { target_indices.emplace_back(idx); - if (max_target_size <= target_indices.size()) { + if (MAX_NUM_TARGET <= target_indices.size()) { break; } } @@ -519,16 +396,17 @@ std::vector MTRNode::extract_target_agents(const std::vector MTRNode::get_relative_timestamps() const +std::vector MTRNode::getRelativeTimestamps() const { - auto output = timestamps_; - for (auto & t : output) { - t -= timestamps_.at(0); + std::vector output; + output.reserve(timestamps_->size()); + for (const auto & t : *timestamps_) { + output.push_back(t - timestamps_->front()); } return output; } -PredictedObject MTRNode::to_predicted_object( +PredictedObject MTRNode::createPredictedObject( const TrackedObject & object, const PredictedTrajectory & trajectory) { const auto & init_pose_with_cov = object.kinematics.pose_with_covariance; @@ -542,20 +420,18 @@ PredictedObject MTRNode::to_predicted_object( predicted_object.classification = object.classification; predicted_object.shape = object.shape; predicted_object.object_id = object.object_id; + predicted_object.existence_probability = object.existence_probability; - float max_existence_probability = 0.0f; for (const auto & mode : trajectory.get_modes()) { PredictedPath waypoints; waypoints.confidence = mode.score(); waypoints.time_step = rclcpp::Duration::from_seconds(0.1); // TODO(ktro282): use a parameter waypoints.path.reserve(mode.num_future()); - if (max_existence_probability < mode.score()) { - max_existence_probability = mode.score(); - } + for (const auto & state : mode.get_waypoints()) { geometry_msgs::msg::Pose predicted_pose; - predicted_pose.position.x = static_cast(state.x()); - predicted_pose.position.y = static_cast(state.y()); + predicted_pose.position.x = state.x(); + predicted_pose.position.y = state.y(); predicted_pose.position.z = init_pose_with_cov.pose.position.z; predicted_pose.orientation = init_pose_with_cov.pose.orientation; waypoints.path.emplace_back(predicted_pose); @@ -565,7 +441,6 @@ PredictedObject MTRNode::to_predicted_object( } predicted_object.kinematics.predicted_paths.emplace_back(waypoints); } - predicted_object.existence_probability = max_existence_probability; return predicted_object; } diff --git a/perception/autoware_mtr/src/trt_mtr.cpp b/perception/autoware_mtr/src/trt_mtr.cpp index d04124656e3a7..1fd47e017ed8d 100644 --- a/perception/autoware_mtr/src/trt_mtr.cpp +++ b/perception/autoware_mtr/src/trt_mtr.cpp @@ -14,17 +14,26 @@ #include "autoware/mtr/trt_mtr.hpp" +#include "autoware/mtr/cuda_helper.hpp" +#include "autoware/mtr/intention_point.hpp" +#include "autoware/mtr/trajectory.hpp" #include "postprocess/postprocess_kernel.cuh" #include "preprocess/agent_preprocess_kernel.cuh" #include "preprocess/polyline_preprocess_kernel.cuh" +#include +#include +#include +#include + namespace autoware::mtr { TrtMTR::TrtMTR( const std::string & model_path, const MTRConfig & config, const BuildConfig & build_config, const size_t max_workspace_size) : config_(config), - intention_point_(config_.intention_point_filepath, config_.num_intention_point_cluster) + intention_point_(IntentionPoint::from_file( + config_.num_intention_point_cluster, config_.intention_point_filepath)) { max_num_polyline_ = config_.max_num_polyline; num_mode_ = config_.num_mode; @@ -141,10 +150,10 @@ void TrtMTR::initCudaPtr(const AgentData & agent_data, const PolylineData & poly bool TrtMTR::preProcess(const AgentData & agent_data, const PolylineData & polyline_data) { CHECK_CUDA_ERROR(cudaMemcpyAsync( - d_target_index_.get(), agent_data.target_index().data(), sizeof(int) * num_target_, + d_target_index_.get(), agent_data.target_indices().data(), sizeof(int) * num_target_, cudaMemcpyHostToDevice, stream_)); CHECK_CUDA_ERROR(cudaMemcpyAsync( - d_label_index_.get(), agent_data.label_index().data(), sizeof(int) * num_agent_, + d_label_index_.get(), agent_data.label_ids().data(), sizeof(int) * num_agent_, cudaMemcpyHostToDevice, stream_)); CHECK_CUDA_ERROR(cudaMemcpyAsync( d_timestamp_.get(), agent_data.timestamps().data(), sizeof(float) * num_timestamp_, @@ -153,22 +162,22 @@ bool TrtMTR::preProcess(const AgentData & agent_data, const PolylineData & polyl d_trajectory_.get(), agent_data.data_ptr(), sizeof(float) * agent_data.size(), cudaMemcpyHostToDevice, stream_)); CHECK_CUDA_ERROR(cudaMemcpyAsync( - d_target_state_.get(), agent_data.target_data_ptr(), + d_target_state_.get(), agent_data.current_target_data_ptr(), sizeof(float) * num_target_ * agent_data.state_dim(), cudaMemcpyHostToDevice, stream_)); CHECK_CUDA_ERROR(cudaMemcpyAsync( d_polyline_.get(), polyline_data.data_ptr(), sizeof(float) * polyline_data.size(), cudaMemcpyHostToDevice, stream_)); - const auto target_label_names = getLabelNames(agent_data.target_label_index()); - const auto intention_point = intention_point_.get_points(target_label_names); + const auto target_label_names = getLabelNames(agent_data.target_label_ids()); + const auto intention_point = intention_point_.as_array(target_label_names); CHECK_CUDA_ERROR(cudaMemcpyAsync( d_intention_point_.get(), intention_point.data(), sizeof(float) * num_target_ * intention_point_.size(), cudaMemcpyHostToDevice, stream_)); CHECK_CUDA_ERROR(agentPreprocessLauncher( num_target_, num_agent_, num_timestamp_, agent_data.state_dim(), agent_data.num_class(), - agent_data.sdc_index(), d_target_index_.get(), d_label_index_.get(), d_timestamp_.get(), + agent_data.ego_index(), d_target_index_.get(), d_label_index_.get(), d_timestamp_.get(), d_trajectory_.get(), d_in_trajectory_.get(), d_in_trajectory_mask_.get(), d_in_last_pos_.get(), stream_)); @@ -194,26 +203,29 @@ bool TrtMTR::postProcess( num_target_, num_mode_, num_future_, agent_data.state_dim(), d_target_state_.get(), PredictedStateDim, d_out_trajectory_.get(), stream_)); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + h_out_score_.clear(); h_out_trajectory_.clear(); - h_out_score_.reserve(num_target_ * num_mode_); - h_out_trajectory_.reserve(num_target_ * num_mode_ * num_future_ * PredictedStateDim); + h_out_score_.resize(num_target_ * num_mode_); + h_out_trajectory_.resize(num_target_ * num_mode_ * num_future_ * PredictedStateDim); - CHECK_CUDA_ERROR(cudaMemcpyAsync( + CHECK_CUDA_ERROR(cudaMemcpy( h_out_score_.data(), d_out_score_.get(), sizeof(float) * num_target_ * num_mode_, - cudaMemcpyDeviceToHost, stream_)); - CHECK_CUDA_ERROR(cudaMemcpyAsync( + cudaMemcpyDeviceToHost)); + CHECK_CUDA_ERROR(cudaMemcpy( h_out_trajectory_.data(), d_out_trajectory_.get(), sizeof(float) * num_target_ * num_mode_ * num_future_ * PredictedStateDim, - cudaMemcpyDeviceToHost, stream_)); + cudaMemcpyDeviceToHost)); + trajectories.clear(); trajectories.reserve(num_target_); for (auto b = 0; b < num_target_; ++b) { const auto score_itr = h_out_score_.cbegin() + b * num_mode_; - std::vector scores(score_itr, score_itr + num_mode_); + const std::vector scores(score_itr, score_itr + num_mode_); const auto mode_itr = h_out_trajectory_.cbegin() + b * num_mode_ * num_future_ * PredictedStateDim; - std::vector modes(mode_itr, mode_itr + num_mode_ * num_future_ * PredictedStateDim); + std::vector modes(mode_itr, mode_itr + num_mode_ * num_future_ * PredictedStateDim); trajectories.emplace_back(scores, modes, num_mode_, num_future_); } return true; diff --git a/perception/autoware_mtr/test/test_fixed_queue.cpp b/perception/autoware_mtr/test/test_fixed_queue.cpp new file mode 100644 index 0000000000000..5ed4ebf5b2a28 --- /dev/null +++ b/perception/autoware_mtr/test/test_fixed_queue.cpp @@ -0,0 +1,42 @@ +// Copyright 2024 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 "autoware/mtr/fixed_queue.hpp" + +#include + +#include + +TEST(testFixedQueue, test_name) +{ + constexpr size_t n_size = 5; + autoware::mtr::FixedQueue queue(n_size); + EXPECT_DOUBLE_EQ(queue.front(), 0.0); + EXPECT_DOUBLE_EQ(queue.back(), 0.0); + EXPECT_EQ(queue.size(), n_size); + + for (size_t n = 0; n < n_size; ++n) { + queue.push_back(n); + } + + EXPECT_DOUBLE_EQ(queue.front(), 0.0); + EXPECT_DOUBLE_EQ(queue.back(), 4.0); + EXPECT_EQ(queue.size(), n_size); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}