diff --git a/.cspell-partial.json b/.cspell-partial.json deleted file mode 100644 index 13849ef298019..0000000000000 --- a/.cspell-partial.json +++ /dev/null @@ -1,9 +0,0 @@ -{ - "ignorePaths": [ - "**/perception/**", - "sensing/tier4_pcl_extensions/include/**", - "perception/bytetrack/lib/**" - ], - "ignoreRegExpList": [], - "words": ["dltype", "tvmgen"] -} diff --git a/.cspell.json b/.cspell.json new file mode 100644 index 0000000000000..c3afe17698a6f --- /dev/null +++ b/.cspell.json @@ -0,0 +1,5 @@ +{ + "ignorePaths": ["perception/bytetrack/lib/**"], + "ignoreRegExpList": [], + "words": ["dltype", "tvmgen"] +} diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index f7603af847d1f..256b617116432 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -77,7 +77,7 @@ evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai fumiya.watanabe@tier4 evaluator/localization_evaluator/** dominik.jargot@robotec.ai koji.minoda@tier4.jp evaluator/perception_online_evaluator/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp evaluator/planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp -evaluator/tier4_metrics_rviz_plugin/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp +evaluator/tier4_metrics_rviz_plugin/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp launch/tier4_localization_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @@ -121,7 +121,7 @@ perception/detected_object_feature_remover/** tomoya.kimura@tier4.jp perception/detected_object_validation/** dai.nguyen@tier4.jp shintaro.tomie@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/detection_by_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp shintaro.tomie@tier4.jp taichi.higashide@tier4.jp -perception/euclidean_cluster/** yukihiro.saito@tier4.jp +perception/euclidean_cluster/** dai.nguyen@tier4.jp yukihiro.saito@tier4.jp perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/image_projection_based_fusion/** dai.nguyen@tier4.jp koji.minoda@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp @@ -212,7 +212,7 @@ sensing/image_diagnostics/** dai.nguyen@tier4.jp sensing/image_transport_decompressor/** yukihiro.saito@tier4.jp sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/livox/livox_tag_filter/** ryohsuke.mitsudome@tier4.jp -sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp +sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp diff --git a/.github/workflows/build-and-test-self-hosted.yaml b/.github/workflows/build-and-test-arm64.yaml similarity index 84% rename from .github/workflows/build-and-test-self-hosted.yaml rename to .github/workflows/build-and-test-arm64.yaml index 9f351a5dc75ec..932c15d27b05f 100644 --- a/.github/workflows/build-and-test-self-hosted.yaml +++ b/.github/workflows/build-and-test-arm64.yaml @@ -1,4 +1,4 @@ -name: build-and-test-self-hosted +name: build-and-test-arm64 on: schedule: @@ -6,7 +6,7 @@ on: workflow_dispatch: jobs: - build-and-test-self-hosted: + build-and-test-arm64: runs-on: [self-hosted, linux, ARM64] container: ${{ matrix.container }}${{ matrix.container-suffix }} strategy: @@ -19,12 +19,15 @@ jobs: - -cuda include: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware-universe:humble-latest + container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt build-depends-repos: build_depends.repos steps: - name: Check out repository uses: actions/checkout@v3 + - name: Show disk space before the tasks + run: df -h + - name: Remove exec_depend uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 @@ -47,3 +50,6 @@ jobs: rosdistro: ${{ matrix.rosdistro }} target-packages: ${{ steps.get-self-packages.outputs.self-packages }} build-depends-repos: ${{ matrix.build-depends-repos }} + + - name: Show disk space after the tasks + run: df -h diff --git a/.github/workflows/build-and-test-differential-self-hosted.yaml b/.github/workflows/build-and-test-differential-arm64.yaml similarity index 85% rename from .github/workflows/build-and-test-differential-self-hosted.yaml rename to .github/workflows/build-and-test-differential-arm64.yaml index 86d3011cf3208..f742dedcf0b89 100644 --- a/.github/workflows/build-and-test-differential-self-hosted.yaml +++ b/.github/workflows/build-and-test-differential-arm64.yaml @@ -1,4 +1,4 @@ -name: build-and-test-differential-self-hosted +name: build-and-test-differential-arm64 on: pull_request: @@ -12,9 +12,9 @@ jobs: prevent-no-label-execution: uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 with: - label: ARM64 + label: type:arm64 - build-and-test-differential-self-hosted: + build-and-test-differential-arm64: needs: prevent-no-label-execution if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} runs-on: [self-hosted, linux, ARM64] @@ -29,7 +29,7 @@ jobs: - -cuda include: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware-universe:humble-latest + container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt build-depends-repos: build_depends.repos steps: - name: Check out repository @@ -37,6 +37,9 @@ jobs: with: fetch-depth: 0 + - name: Show disk space before the tasks + run: df -h + - name: Remove exec_depend uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 @@ -59,3 +62,6 @@ jobs: rosdistro: ${{ matrix.rosdistro }} target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} build-depends-repos: ${{ matrix.build-depends-repos }} + + - name: Show disk space after the tasks + run: df -h diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index e4231a12a6add..0e759fe47630b 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -16,7 +16,7 @@ jobs: build-and-test-differential: needs: prevent-no-label-execution if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} - runs-on: [self-hosted, linux, X64] + runs-on: ubuntu-latest container: ${{ matrix.container }}${{ matrix.container-suffix }} strategy: fail-fast: false @@ -24,10 +24,11 @@ jobs: rosdistro: - humble container-suffix: + - "" - -cuda include: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware-universe:humble-latest + container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt build-depends-repos: build_depends.repos steps: - name: Check out repository @@ -35,7 +36,7 @@ jobs: with: fetch-depth: 0 - - name: Check disk space before build + - name: Show disk space before the tasks run: df -h - name: Remove exec_depend @@ -71,5 +72,42 @@ jobs: verbose: true flags: differential - - name: Check disk space after build + - name: Show disk space after the tasks run: df -h + + clang-tidy-differential: + runs-on: ubuntu-latest + container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt-cuda + steps: + - name: Check out repository + uses: actions/checkout@v3 + with: + fetch-depth: 0 + + - name: Show disk space before the tasks + run: df -h + + - name: Remove exec_depend + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 + + - name: Get modified packages + id: get-modified-packages + uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 + + - name: Get modified files + id: get-modified-files + uses: tj-actions/changed-files@v35 + with: + files: | + **/*.cpp + **/*.hpp + + - name: Run clang-tidy + if: ${{ steps.get-modified-files.outputs.all_changed_files != '' }} + uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 + with: + rosdistro: humble + target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} + target-files: ${{ steps.get-modified-files.outputs.all_changed_files }} + clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy + build-depends-repos: build_depends.repos diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index a6eea766cd80d..49e1c979928ad 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -9,7 +9,7 @@ on: jobs: build-and-test: if: ${{ github.event_name != 'push' || github.ref_name == github.event.repository.default_branch }} - runs-on: [self-hosted, linux, X64] + runs-on: ubuntu-latest container: ${{ matrix.container }}${{ matrix.container-suffix }} strategy: fail-fast: false @@ -21,12 +21,15 @@ jobs: - -cuda include: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware-universe:humble-latest + container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt build-depends-repos: build_depends.repos steps: - name: Check out repository uses: actions/checkout@v3 + - name: Show disk space before the tasks + run: df -h + - name: Remove exec_depend uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 @@ -59,3 +62,6 @@ jobs: fail_ci_if_error: false verbose: true flags: total + + - name: Show disk space after the tasks + run: df -h diff --git a/.github/workflows/pr-agent.yaml b/.github/workflows/pr-agent.yaml new file mode 100644 index 0000000000000..f4794b7497402 --- /dev/null +++ b/.github/workflows/pr-agent.yaml @@ -0,0 +1,31 @@ +name: PR-Agent + +on: + pull_request: + types: [opened, labeled, unlabeled, synchronize] + issue_comment: + +jobs: + prevent-no-label-execution-pr-agent: + uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 + with: + label: tag:pr-agent + pr_agent_job: + needs: prevent-no-label-execution-pr-agent + if: ${{ needs.prevent-no-label-execution-pr-agent.outputs.run == 'true' }} + runs-on: ubuntu-latest + permissions: + issues: write + pull-requests: write + contents: write + name: Run pr agent on every pull request, respond to user comments + steps: + - name: PR Agent action step + id: pragent + uses: Codium-ai/pr-agent@main + env: + OPENAI_KEY: ${{ secrets.OPENAI_KEY_PR_AGENT }} + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + github_action_config.auto_review: "false" + github_action_config.auto_describe: "false" + github_action_config.auto_improve: "false" diff --git a/.github/workflows/spell-check-all.yaml b/.github/workflows/spell-check-all.yaml deleted file mode 100644 index 057bd78da2a82..0000000000000 --- a/.github/workflows/spell-check-all.yaml +++ /dev/null @@ -1,19 +0,0 @@ -name: spell-check-all - -on: - workflow_dispatch: - schedule: - - cron: 0 0 * * * - -jobs: - spell-check-all: - runs-on: ubuntu-latest - steps: - - name: Check out repository - uses: actions/checkout@v3 - - - name: Run spell-check - uses: autowarefoundation/autoware-github-actions/spell-check@v1 - with: - cspell-json-url: https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/.cspell.json - incremental-files-only: false diff --git a/.github/workflows/spell-check-partial.yaml b/.github/workflows/spell-check-partial.yaml index 608e7644b47b9..cf5eaa71e5741 100644 --- a/.github/workflows/spell-check-partial.yaml +++ b/.github/workflows/spell-check-partial.yaml @@ -2,17 +2,20 @@ name: spell-check-partial on: pull_request: + schedule: + - cron: 0 0 * * * + workflow_dispatch: jobs: spell-check-partial: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Run spell-check uses: autowarefoundation/autoware-github-actions/spell-check@v1 with: cspell-json-url: https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/.cspell.json - local-cspell-json: .cspell-partial.json + local-cspell-json: .cspell.json incremental-files-only: false diff --git a/build_depends.repos b/build_depends.repos index 1193d710b91f4..f7b3f12484015 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -16,6 +16,10 @@ repositories: type: git url: https://github.com/autowarefoundation/autoware_adapi_msgs.git version: main + core/autoware_internal_msgs: + type: git + url: https://github.com/autowarefoundation/autoware_internal_msgs.git + version: main core/external/autoware_auto_msgs: type: git url: https://github.com/tier4/autoware_auto_msgs.git diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp new file mode 100644 index 0000000000000..21705cab9a962 --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp @@ -0,0 +1,114 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 TIER4_AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_ +#define TIER4_AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_ + +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace tier4_autoware_utils +{ + +class PublishedTimePublisher +{ +public: + explicit PublishedTimePublisher( + rclcpp::Node * node, std::string publisher_topic_suffix = "/debug/published_time", + const rclcpp::QoS & qos = rclcpp::QoS(1)) + : node_(node), publisher_topic_suffix_(std::move(publisher_topic_suffix)), qos_(qos) + { + } + + void publish_if_subscribed( + const rclcpp::PublisherBase::ConstSharedPtr & publisher, const rclcpp::Time & stamp) + { + const auto & gid_key = publisher->get_gid(); + + // if the publisher is not in the map, create a new publisher for published time + ensure_publisher_exists(gid_key, publisher->get_topic_name()); + + const auto & pub_published_time = publishers_[gid_key]; + + // Check if there are any subscribers, otherwise don't do anything + if (pub_published_time->get_subscription_count() > 0) { + PublishedTime published_time; + + published_time.header.stamp = stamp; + published_time.published_stamp = rclcpp::Clock().now(); + + pub_published_time->publish(published_time); + } + } + + void publish_if_subscribed( + const rclcpp::PublisherBase::ConstSharedPtr & publisher, const std_msgs::msg::Header & header) + { + const auto & gid_key = publisher->get_gid(); + + // if the publisher is not in the map, create a new publisher for published time + ensure_publisher_exists(gid_key, publisher->get_topic_name()); + + const auto & pub_published_time = publishers_[gid_key]; + + // Check if there are any subscribers, otherwise don't do anything + if (pub_published_time->get_subscription_count() > 0) { + PublishedTime published_time; + + published_time.header = header; + published_time.published_stamp = rclcpp::Clock().now(); + + pub_published_time->publish(published_time); + } + } + +private: + rclcpp::Node * node_; + std::string publisher_topic_suffix_; + rclcpp::QoS qos_; + + using PublishedTime = autoware_internal_msgs::msg::PublishedTime; + + // Custom comparison struct for rmw_gid_t + struct GidCompare + { + bool operator()(const rmw_gid_t & lhs, const rmw_gid_t & rhs) const + { + return std::memcmp(lhs.data, rhs.data, RMW_GID_STORAGE_SIZE) < 0; + } + }; + + // ensure that the publisher exists in publisher_ map, if not, create a new one + void ensure_publisher_exists(const rmw_gid_t & gid_key, const std::string & topic_name) + { + if (publishers_.find(gid_key) == publishers_.end()) { + publishers_[gid_key] = + node_->create_publisher(topic_name + publisher_topic_suffix_, qos_); + } + } + + // store them for each different publisher of the node + std::map::SharedPtr, GidCompare> publishers_; +}; +} // namespace tier4_autoware_utils + +#endif // TIER4_AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_ diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index 04a07b88edf4e..c34d3b5fdfdd0 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -15,6 +15,7 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs + autoware_internal_msgs builtin_interfaces diagnostic_msgs geometry_msgs diff --git a/common/tier4_autoware_utils/test/src/ros/test_published_time_publisher.cpp b/common/tier4_autoware_utils/test/src/ros/test_published_time_publisher.cpp new file mode 100644 index 0000000000000..133cb01f9b348 --- /dev/null +++ b/common/tier4_autoware_utils/test/src/ros/test_published_time_publisher.cpp @@ -0,0 +1,272 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 +#include + +#include +#include + +#include + +class PublishedTimePublisherWithSubscriptionTest : public ::testing::Test +{ +protected: + std::shared_ptr node_{nullptr}; + std::shared_ptr published_time_publisher_ptr_{ + nullptr}; + + std::shared_ptr> first_test_publisher_ptr_{nullptr}; + std::shared_ptr> second_test_publisher_ptr_{nullptr}; + + std::shared_ptr> + first_test_subscriber_ptr_{nullptr}; + std::shared_ptr> + second_test_subscriber_ptr_{nullptr}; + + autoware_internal_msgs::msg::PublishedTime::ConstSharedPtr first_published_time_ptr_{nullptr}; + autoware_internal_msgs::msg::PublishedTime::ConstSharedPtr second_published_time_ptr_{nullptr}; + + std_msgs::msg::Header first_header_; + std_msgs::msg::Header second_header_; + + void SetUp() override + { + // Simplify node and topic names for brevity and uniqueness + const std::string test_name = ::testing::UnitTest::GetInstance()->current_test_info()->name(); + const std::string base_name = + "published_time_publisher_" + test_name; // Base name for node and topics + const std::string suffix = "/debug/published_time"; // Suffix for published time topics + + // Initialize ROS node + node_ = std::make_shared(base_name + "_node"); + + ASSERT_TRUE(rclcpp::ok()); + + // init headers which will be used to publish + first_header_.stamp = rclcpp::Time(0); + first_header_.frame_id = "frame_id_1"; + second_header_.stamp = rclcpp::Time(1); + second_header_.frame_id = "frame_id_2"; + + // Create the first publisher + first_test_publisher_ptr_ = + node_->create_publisher(base_name + "_topic1", 1); + + // Create the second publisher + second_test_publisher_ptr_ = + node_->create_publisher(base_name + "_topic2", 1); + + // Create a PublishedTimePublisher + published_time_publisher_ptr_ = + std::make_shared(node_.get()); + + // Create the first subscriber + first_test_subscriber_ptr_ = + node_->create_subscription( + base_name + "_topic1" + suffix, 1, + [this](autoware_internal_msgs::msg::PublishedTime::ConstSharedPtr msg) { + this->first_published_time_ptr_ = std::move(msg); + }); + + // Create the second subscriber + second_test_subscriber_ptr_ = + node_->create_subscription( + base_name + "_topic2" + suffix, 1, + [this](autoware_internal_msgs::msg::PublishedTime::ConstSharedPtr msg) { + this->second_published_time_ptr_ = std::move(msg); + }); + + rclcpp::spin_some(node_); + } + + void TearDown() override {} +}; + +class PublishedTimePublisherWithoutSubscriptionTest : public ::testing::Test +{ +protected: + std::shared_ptr node_{nullptr}; + std::shared_ptr published_time_publisher_ptr_{ + nullptr}; + + std::shared_ptr> first_test_publisher_ptr_{nullptr}; + std::shared_ptr> second_test_publisher_ptr_{nullptr}; + + std_msgs::msg::Header first_header_; + std_msgs::msg::Header second_header_; + + void SetUp() override + { + // Simplify node and topic names for brevity and uniqueness + const std::string test_name = ::testing::UnitTest::GetInstance()->current_test_info()->name(); + const std::string base_name = + "published_time_publisher_" + test_name; // Base name for node and topics + + // Initialize ROS node + node_ = std::make_shared(base_name + "_node"); + + ASSERT_TRUE(rclcpp::ok()); + + // init headers which will be used to publish + first_header_.stamp = rclcpp::Time(0); + first_header_.frame_id = "frame_id_1"; + second_header_.stamp = rclcpp::Time(1); + second_header_.frame_id = "frame_id_2"; + + // Create the first publisher + first_test_publisher_ptr_ = + node_->create_publisher(base_name + "_topic1", 1); + + // Create the second publisher + second_test_publisher_ptr_ = + node_->create_publisher(base_name + "_topic2", 1); + + // Create a PublishedTimePublisher + published_time_publisher_ptr_ = + std::make_shared(node_.get()); + + rclcpp::spin_some(node_); + } + + void TearDown() override {} +}; + +TEST_F(PublishedTimePublisherWithSubscriptionTest, PublishMsgWithHeader) +{ + // Check if the PublishedTimePublisher is created + ASSERT_TRUE(published_time_publisher_ptr_ != nullptr); + + // Use Published Time Publisher .publish_if_subscribed() with a header + published_time_publisher_ptr_->publish_if_subscribed(first_test_publisher_ptr_, first_header_); + rclcpp::spin_some(node_); + + // Check if the published_time_ is created + ASSERT_TRUE(first_published_time_ptr_ != nullptr); + + // Check if the published time is the same as the header + EXPECT_EQ(first_published_time_ptr_->header.stamp, first_header_.stamp); + EXPECT_EQ(first_published_time_ptr_->header.frame_id, first_header_.frame_id); +} + +TEST_F(PublishedTimePublisherWithSubscriptionTest, PublishMsgWithTimestamp) +{ + // Check if the PublishedTimePublisher is created + ASSERT_TRUE(published_time_publisher_ptr_ != nullptr); + + // Use Published Time Publisher .publish_if_subscribed() with a timestamp + published_time_publisher_ptr_->publish_if_subscribed( + first_test_publisher_ptr_, first_header_.stamp); + rclcpp::spin_some(node_); + + // Check if the published_time_ is created + ASSERT_TRUE(first_published_time_ptr_ != nullptr); + + // Check if the published time is the same as the header + EXPECT_EQ(first_published_time_ptr_->header.stamp, first_header_.stamp); + EXPECT_EQ(first_published_time_ptr_->header.frame_id, std::string("")); +} + +TEST_F(PublishedTimePublisherWithSubscriptionTest, MultiplePublishMsgWithHeader) +{ + // Check if the PublishedTimePublisher is created + ASSERT_TRUE(published_time_publisher_ptr_ != nullptr); + + // Use Published Time Publisher .publish_if_subscribed() with a header for multiple publishers + published_time_publisher_ptr_->publish_if_subscribed(first_test_publisher_ptr_, first_header_); + published_time_publisher_ptr_->publish_if_subscribed(second_test_publisher_ptr_, second_header_); + rclcpp::spin_some(node_); + + // Check if the published_time_ is created + ASSERT_TRUE(first_published_time_ptr_ != nullptr); + ASSERT_TRUE(second_published_time_ptr_ != nullptr); + + // Check if the published time is the same as the header + EXPECT_EQ(first_published_time_ptr_->header.stamp, first_header_.stamp); + EXPECT_EQ(second_published_time_ptr_->header.stamp, second_header_.stamp); + EXPECT_EQ(first_published_time_ptr_->header.frame_id, first_header_.frame_id); + EXPECT_EQ(second_published_time_ptr_->header.frame_id, second_header_.frame_id); +} + +TEST_F(PublishedTimePublisherWithSubscriptionTest, MultiplePublishMsgWithTimestamp) +{ + // Check if the PublishedTimePublisher is created + ASSERT_TRUE(published_time_publisher_ptr_ != nullptr); + + // Use Published Time Publisher .publish_if_subscribed() with a timestamp for multiple publishers + published_time_publisher_ptr_->publish_if_subscribed( + first_test_publisher_ptr_, first_header_.stamp); + published_time_publisher_ptr_->publish_if_subscribed( + second_test_publisher_ptr_, second_header_.stamp); + rclcpp::spin_some(node_); + + // Check if the published_time_ is created + ASSERT_TRUE(first_published_time_ptr_ != nullptr); + ASSERT_TRUE(second_published_time_ptr_ != nullptr); + + // Check if the published time is the same as the header + EXPECT_EQ(first_published_time_ptr_->header.stamp, first_header_.stamp); + EXPECT_EQ(second_published_time_ptr_->header.stamp, second_header_.stamp); + EXPECT_EQ(first_published_time_ptr_->header.frame_id, std::string("")); + EXPECT_EQ(second_published_time_ptr_->header.frame_id, std::string("")); +} + +TEST_F(PublishedTimePublisherWithoutSubscriptionTest, PublishMsgWithHeader) +{ + // Check if the PublishedTimePublisher is created + ASSERT_TRUE(published_time_publisher_ptr_ != nullptr); + + // Use Published Time Publisher .publish_if_subscribed() with a header + published_time_publisher_ptr_->publish_if_subscribed(first_test_publisher_ptr_, first_header_); + rclcpp::spin_some(node_); + ASSERT_TRUE(rclcpp::ok()); +} + +TEST_F(PublishedTimePublisherWithoutSubscriptionTest, PublishMsgWithTimestamp) +{ + // Check if the PublishedTimePublisher is created + ASSERT_TRUE(published_time_publisher_ptr_ != nullptr); + + // Use Published Time Publisher .publish_if_subscribed() with a timestamp + published_time_publisher_ptr_->publish_if_subscribed( + first_test_publisher_ptr_, first_header_.stamp); + rclcpp::spin_some(node_); + ASSERT_TRUE(rclcpp::ok()); +} + +TEST_F(PublishedTimePublisherWithoutSubscriptionTest, MultiplePublishMsgWithHeader) +{ + // Check if the PublishedTimePublisher is created + ASSERT_TRUE(published_time_publisher_ptr_ != nullptr); + + // Use Published Time Publisher .publish_if_subscribed() with a header for multiple publishers + published_time_publisher_ptr_->publish_if_subscribed(first_test_publisher_ptr_, first_header_); + published_time_publisher_ptr_->publish_if_subscribed(second_test_publisher_ptr_, second_header_); + rclcpp::spin_some(node_); + ASSERT_TRUE(rclcpp::ok()); +} + +TEST_F(PublishedTimePublisherWithoutSubscriptionTest, MultiplePublishMsgWithTimestamp) +{ + // Check if the PublishedTimePublisher is created + ASSERT_TRUE(published_time_publisher_ptr_ != nullptr); + + // Use Published Time Publisher .publish_if_subscribed() with a timestamp for multiple publishers + published_time_publisher_ptr_->publish_if_subscribed( + first_test_publisher_ptr_, first_header_.stamp); + published_time_publisher_ptr_->publish_if_subscribed( + second_test_publisher_ptr_, second_header_.stamp); + rclcpp::spin_some(node_); + ASSERT_TRUE(rclcpp::ok()); +} diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index be2411cd3268b..c658cf4497973 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -121,7 +121,7 @@ class LaneDepartureChecker std::vector> getLaneletsFromPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; - std::optional getFusedLaneletPolygonForPath( + std::optional getFusedLaneletPolygonForPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; bool checkPathWillLeaveLane( diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index 811e1652fcb4a..c48383a17ab4b 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -321,33 +321,39 @@ std::vector> LaneDepartureChecker::getLanele lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0); } -std::optional LaneDepartureChecker::getFusedLaneletPolygonForPath( +std::optional LaneDepartureChecker::getFusedLaneletPolygonForPath( const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const { const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path); + auto to_polygon2d = [](const lanelet::BasicPolygon2d & poly) -> tier4_autoware_utils::Polygon2d { + tier4_autoware_utils::Polygon2d p; + auto & outer = p.outer(); + + for (const auto & p : poly) { + tier4_autoware_utils::Point2d p2d(p.x(), p.y()); + outer.push_back(p2d); + } + boost::geometry::correct(p); + return p; + }; + // Fuse lanelets into a single BasicPolygon2d - auto fused_lanelets = [&lanelets_distance_pair]() -> std::optional { + auto fused_lanelets = [&]() -> std::optional { if (lanelets_distance_pair.empty()) return std::nullopt; - if (lanelets_distance_pair.size() == 1) - return lanelets_distance_pair.at(0).second.polygon2d().basicPolygon(); + tier4_autoware_utils::MultiPolygon2d lanelet_unions; + tier4_autoware_utils::MultiPolygon2d result; - lanelet::BasicPolygon2d merged_polygon = - lanelets_distance_pair.at(0).second.polygon2d().basicPolygon(); - for (size_t i = 1; i < lanelets_distance_pair.size(); ++i) { + for (size_t i = 0; i < lanelets_distance_pair.size(); ++i) { const auto & route_lanelet = lanelets_distance_pair.at(i).second; - const auto & poly = route_lanelet.polygon2d().basicPolygon(); - - std::vector lanelet_union_temp; - boost::geometry::union_(poly, merged_polygon, lanelet_union_temp); - - // Update merged_polygon by accumulating all merged results - merged_polygon.clear(); - for (const auto & temp_poly : lanelet_union_temp) { - merged_polygon.insert(merged_polygon.end(), temp_poly.begin(), temp_poly.end()); - } + const auto & p = route_lanelet.polygon2d().basicPolygon(); + tier4_autoware_utils::Polygon2d poly = to_polygon2d(p); + boost::geometry::union_(lanelet_unions, poly, result); + lanelet_unions = result; + result.clear(); } - if (merged_polygon.empty()) return std::nullopt; - return merged_polygon; + + if (lanelet_unions.empty()) return std::nullopt; + return lanelet_unions.front(); }(); return fused_lanelets; diff --git a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml index 182458d532c8a..c0382ca2ccb6b 100644 --- a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml +++ b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml @@ -1,58 +1,58 @@ /**: ros__parameters: # -- system -- - traj_resample_dist: 0.1 # path resampling interval [m] - use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) - admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value - admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value + traj_resample_dist: 0.1 # path resampling interval [m] + use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) + admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value + admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value # -- path smoothing -- - enable_path_smoothing: false # flag for path smoothing + enable_path_smoothing: false # flag for path smoothing path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing - curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num) - curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num) + curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num) + curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num) # -- trajectory extending -- - extend_trajectory_for_end_yaw_control: true # flag of trajectory extending for terminal yaw control + extend_trajectory_for_end_yaw_control: false # flag of trajectory extending for terminal yaw control # -- mpc optimization -- - qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp) - mpc_prediction_horizon: 50 # prediction horizon step - mpc_prediction_dt: 0.1 # prediction horizon period [s] - mpc_weight_lat_error: 0.1 # lateral error weight in matrix Q - mpc_weight_heading_error: 0.0 # heading error weight in matrix Q - mpc_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q - mpc_weight_steering_input: 1.0 # steering error weight in matrix R - mpc_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R - mpc_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R - mpc_weight_steer_rate: 0.0 # steering rate weight in matrix R - mpc_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R - mpc_low_curvature_weight_lat_error: 0.1 # lateral error weight in matrix Q in low curvature point - mpc_low_curvature_weight_heading_error: 0.0 # heading error weight in matrix Q in low curvature point - mpc_low_curvature_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q in low curvature point - mpc_low_curvature_weight_steering_input: 1.0 # steering error weight in matrix R in low curvature point - mpc_low_curvature_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R in low curvature point - mpc_low_curvature_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R in low curvature point - mpc_low_curvature_weight_steer_rate: 0.0 # steering rate weight in matrix R in low curvature point - mpc_low_curvature_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R in low curvature point - mpc_low_curvature_thresh_curvature: 0.0 # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03) - mpc_weight_terminal_lat_error: 1.0 # terminal lateral error weight in matrix Q to improve mpc stability - mpc_weight_terminal_heading_error: 0.1 # terminal heading error weight in matrix Q to improve mpc stability - mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero - mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration - mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing - mpc_min_prediction_length: 5.0 # minimum prediction length + qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp) + mpc_prediction_horizon: 50 # prediction horizon step + mpc_prediction_dt: 0.1 # prediction horizon period [s] + mpc_weight_lat_error: 1.0 # lateral error weight in matrix Q + mpc_weight_heading_error: 0.0 # heading error weight in matrix Q + mpc_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q + mpc_weight_steering_input: 1.0 # steering error weight in matrix R + mpc_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R + mpc_weight_lat_jerk: 0.1 # lateral jerk weight in matrix R + mpc_weight_steer_rate: 0.0 # steering rate weight in matrix R + mpc_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R + mpc_low_curvature_weight_lat_error: 0.1 # lateral error weight in matrix Q in low curvature point + mpc_low_curvature_weight_heading_error: 0.0 # heading error weight in matrix Q in low curvature point + mpc_low_curvature_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q in low curvature point + mpc_low_curvature_weight_steering_input: 1.0 # steering error weight in matrix R in low curvature point + mpc_low_curvature_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R in low curvature point + mpc_low_curvature_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R in low curvature point + mpc_low_curvature_weight_steer_rate: 0.0 # steering rate weight in matrix R in low curvature point + mpc_low_curvature_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R in low curvature point + mpc_low_curvature_thresh_curvature: 0.0 # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03) + mpc_weight_terminal_lat_error: 1.0 # terminal lateral error weight in matrix Q to improve mpc stability + mpc_weight_terminal_heading_error: 0.1 # terminal heading error weight in matrix Q to improve mpc stability + mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero + mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration + mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing + mpc_min_prediction_length: 5.0 # minimum prediction length # -- vehicle model -- vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics - input_delay: 0.24 # steering input delay time for delay compensation - vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s] - steer_rate_lim_dps_list_by_curvature: [10.0, 20.0, 30.0] # steering angle rate limit list depending on curvature [deg/s] + input_delay: 0.24 # steering input delay time for delay compensation + vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s] + steer_rate_lim_dps_list_by_curvature: [40.0, 50.0, 60.0] # steering angle rate limit list depending on curvature [deg/s] curvature_list_for_steer_rate_lim: [0.001, 0.002, 0.01] # curvature list for steering angle rate limit interpolation in ascending order [/m] - steer_rate_lim_dps_list_by_velocity: [40.0, 30.0, 20.0] # steering angle rate limit list depending on velocity [deg/s] + steer_rate_lim_dps_list_by_velocity: [60.0, 50.0, 40.0] # steering angle rate limit list depending on velocity [deg/s] velocity_list_for_steer_rate_lim: [10.0, 15.0, 20.0] # velocity list for steering angle rate limit interpolation in ascending order [m/s] - acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss] - velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s] + acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss] + velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s] # -- lowpass filter for noise reduction -- steering_lpf_cutoff_hz: 3.0 # cutoff frequency of lowpass filter for steering command [Hz] @@ -65,7 +65,7 @@ keep_steer_control_until_converged: true new_traj_duration_time: 1.0 new_traj_end_dist: 0.3 - mpc_converged_threshold_rps: 0.01 # threshold of mpc convergence check [rad/s] + mpc_converged_threshold_rps: 0.01 # threshold of mpc convergence check [rad/s] # steer offset steering_offset: diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 8fa95f4b3b87b..373de2e0bd911 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -355,12 +355,15 @@ std::pair MPC::updateStateForDelayCompensation( MatrixXd Wd(DIM_X, 1); MatrixXd Cd(DIM_Y, DIM_X); + const double sign_vx = m_is_forward_shift ? 1 : -1; + MatrixXd x_curr = x0_orig; double mpc_curr_time = start_time; for (size_t i = 0; i < m_input_buffer.size(); ++i) { double k, v = 0.0; try { - k = interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time); + // NOTE: When driving backward, the curvature's sign should be reversed. + k = interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time) * sign_vx; v = interpolation::lerp(traj.relative_time, traj.vx, mpc_curr_time); } catch (const std::exception & e) { RCLCPP_ERROR(m_logger, "mpc resample failed at delay compensation, stop mpc: %s", e.what()); @@ -446,6 +449,7 @@ MPCMatrix MPC::generateMPCMatrix( const double ref_vx = reference_trajectory.vx.at(i); const double ref_vx_squared = ref_vx * ref_vx; + // NOTE: When driving backward, the curvature's sign should be reversed. const double ref_k = reference_trajectory.k.at(i) * sign_vx; const double ref_smooth_k = reference_trajectory.smooth_k.at(i) * sign_vx; diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index 7eca1481ba921..882150ffc1644 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -243,7 +243,11 @@ trajectory_follower::LateralOutput MpcLateralController::run( Trajectory predicted_traj; Float32MultiArrayStamped debug_values; - if (!m_is_ctrl_cmd_prev_initialized) { + const bool is_under_control = input_data.current_operation_mode.is_autoware_control_enabled && + input_data.current_operation_mode.mode == + autoware_adapi_v1_msgs::msg::OperationModeState::AUTONOMOUS; + + if (!m_is_ctrl_cmd_prev_initialized || !is_under_control) { m_ctrl_cmd_prev = getInitialControlCommand(); m_is_ctrl_cmd_prev_initialized = true; } diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 8df7cb3b9b3f8..967b7c40fbd10 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -25,6 +25,7 @@ #include "tf2/utils.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" +#include "tier4_autoware_utils/ros/marker_helper.hpp" #include "trajectory_follower_base/longitudinal_controller_base.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -39,6 +40,7 @@ #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "visualization_msgs/msg/marker.hpp" #include #include @@ -50,6 +52,11 @@ namespace autoware::motion::control::pid_longitudinal_controller { using autoware_adapi_v1_msgs::msg::OperationModeState; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerScale; +using visualization_msgs::msg::Marker; + namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; /// \class PidLongitudinalController @@ -97,6 +104,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // ros variables rclcpp::Publisher::SharedPtr m_pub_slope; rclcpp::Publisher::SharedPtr m_pub_debug; + rclcpp::Publisher::SharedPtr m_pub_stop_reason_marker; rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res; rcl_interfaces::msg::SetParametersResult paramCallback( @@ -109,7 +117,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro OperationModeState m_current_operation_mode; // vehicle info - double m_wheel_base; + double m_wheel_base{0.0}; + double m_vehicle_width{0.0}; + double m_front_overhang{0.0}; bool m_prev_vehicle_is_under_control{false}; std::shared_ptr m_under_control_starting_time{nullptr}; diff --git a/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml b/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml index e95428096c17c..ad6217663fbae 100644 --- a/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml +++ b/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml @@ -5,19 +5,19 @@ enable_smooth_stop: true enable_overshoot_emergency: true enable_large_tracking_error_emergency: true - enable_slope_compensation: false + enable_slope_compensation: true enable_keep_stopped_until_steer_convergence: true # state transition drive_state_stop_dist: 0.5 drive_state_offset_stop_dist: 1.0 - stopping_state_stop_dist: 0.49 + stopping_state_stop_dist: 0.5 stopped_state_entry_duration_time: 0.1 - stopped_state_entry_vel: 0.1 + stopped_state_entry_vel: 0.01 stopped_state_entry_acc: 0.1 emergency_state_overshoot_stop_dist: 1.5 emergency_state_traj_trans_dev: 3.0 - emergency_state_traj_rot_dev: 0.7 + emergency_state_traj_rot_dev: 0.7854 # drive state kp: 1.0 @@ -40,7 +40,7 @@ # smooth stop state smooth_stop_max_strong_acc: -0.5 - smooth_stop_min_strong_acc: -1.0 + smooth_stop_min_strong_acc: -0.8 smooth_stop_weak_acc: -0.3 smooth_stop_weak_stop_acc: -0.8 smooth_stop_strong_stop_acc: -3.4 diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index aab2ecf8f081e..cc1c0c6383707 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -39,6 +39,8 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) m_longitudinal_ctrl_period = node.get_parameter("ctrl_period").as_double(); m_wheel_base = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().wheel_base_m; + m_vehicle_width = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().vehicle_width_m; + m_front_overhang = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().front_overhang_m; // parameters for delay compensation m_delay_compensation_time = node.declare_parameter("delay_compensation_time"); // [s] @@ -204,6 +206,7 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) "~/output/slope_angle", rclcpp::QoS{1}); m_pub_debug = node.create_publisher( "~/output/longitudinal_diagnostic", rclcpp::QoS{1}); + m_pub_stop_reason_marker = node.create_publisher("~/output/stop_reason", rclcpp::QoS{1}); // set parameter callback m_set_param_res = node.add_on_set_parameters_callback( @@ -597,6 +600,16 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d const bool keep_stopped_condition = std::fabs(current_vel) < vel_epsilon && m_enable_keep_stopped_until_steer_convergence && !lateral_sync_data_.is_steer_converged; + if (keep_stopped_condition) { + auto marker = createDefaultMarker( + "map", clock_->now(), "stop_reason", 0, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.pose = tier4_autoware_utils::calcOffsetPose( + m_current_kinematic_state.pose.pose, m_wheel_base + m_front_overhang, + m_vehicle_width / 2 + 2.0, 1.5); + marker.text = "steering not\nconverged"; + m_pub_stop_reason_marker->publish(marker); + } const bool stopping_condition = stop_dist < p.stopping_state_stop_dist; @@ -662,7 +675,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d if (emergency_condition) { return changeState(ControlState::EMERGENCY); } - if (!is_under_control && stopped_condition && keep_stopped_condition) { // NOTE: When the ego is stopped on manual driving, since the driving state may transit to // autonomous, keep_stopped_condition should be checked. @@ -692,7 +704,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d if (emergency_condition) { return changeState(ControlState::EMERGENCY); } - if (stopped_condition) { return changeState(ControlState::STOPPED); } diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp index cc43da7114630..be4fe885ae8c1 100644 --- a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp @@ -28,6 +28,7 @@ #include #include +#include #include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" #include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" @@ -121,6 +122,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node std::unique_ptr logger_configure_; + std::unique_ptr published_time_publisher_; + void publishProcessingTime( const double t_ms, const rclcpp::Publisher::SharedPtr pub); StopWatch stop_watch_; diff --git a/control/trajectory_follower_node/package.xml b/control/trajectory_follower_node/package.xml index 34e9fb45424a7..36b4d0108de78 100644 --- a/control/trajectory_follower_node/package.xml +++ b/control/trajectory_follower_node/package.xml @@ -30,6 +30,7 @@ pure_pursuit rclcpp rclcpp_components + tier4_autoware_utils trajectory_follower_base vehicle_info_util visualization_msgs diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp index 322aa9eef5a79..801f42ad9a470 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -91,6 +91,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control } logger_configure_ = std::make_unique(this); + + published_time_publisher_ = std::make_unique(this); } Controller::LateralControllerMode Controller::getLateralControllerMode( @@ -231,7 +233,8 @@ void Controller::callbackTimerControl() out.longitudinal = lon_out.control_cmd; control_cmd_pub_->publish(out); - // 6. publish debug marker + // 6. publish debug + published_time_publisher_->publish_if_subscribed(control_cmd_pub_, out.stamp); publishDebugMarker(*input_data, lat_out); } diff --git a/control/trajectory_follower_node/test/test_controller_node.cpp b/control/trajectory_follower_node/test/test_controller_node.cpp index 6be3625501590..1aa4035e98d51 100644 --- a/control/trajectory_follower_node/test/test_controller_node.cpp +++ b/control/trajectory_follower_node/test/test_controller_node.cpp @@ -56,8 +56,6 @@ rclcpp::NodeOptions makeNodeOptions(const bool enable_keep_stopped_until_steer_c const auto lateral_share_dir = ament_index_cpp::get_package_share_directory("mpc_lateral_controller"); rclcpp::NodeOptions node_options; - node_options.append_parameter_override("ctrl_period", 0.03); - node_options.append_parameter_override("timeout_thr_sec", 0.5); node_options.append_parameter_override("lateral_controller_mode", "mpc"); node_options.append_parameter_override("longitudinal_controller_mode", "pid"); node_options.append_parameter_override( @@ -254,8 +252,10 @@ TEST_F(FakeNodeFixture, straight_trajectory) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); + // following conditions will pass even if the MPC solution does not converge EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_angle, 0.0f); EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); + EXPECT_GT(tester.cmd_msg->longitudinal.speed, 0.0f); EXPECT_GT(rclcpp::Time(tester.cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); } diff --git a/control/trajectory_follower_node/test/test_lateral_controller_node.cpp b/control/trajectory_follower_node/test/test_lateral_controller_node.cpp deleted file mode 100644 index 0c0a1f84ab1c2..0000000000000 --- a/control/trajectory_follower_node/test/test_lateral_controller_node.cpp +++ /dev/null @@ -1,436 +0,0 @@ -// Copyright 2021 The Autoware Foundation -// -// 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 "ament_index_cpp/get_package_share_directory.hpp" -#include "fake_test_node/fake_test_node.hpp" -#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/time.hpp" -#include "trajectory_follower_test_utils.hpp" - -#include - -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" - -#include -#include - -using LateralController = autoware::motion::control::trajectory_follower_node::LateralController; -using LateralCommand = autoware_auto_control_msgs::msg::AckermannLateralCommand; -using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; -using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; -using VehicleOdometry = nav_msgs::msg::Odometry; -using SteeringReport = autoware_auto_vehicle_msgs::msg::SteeringReport; - -using FakeNodeFixture = autoware::tools::testing::FakeTestNode; - -const rclcpp::Duration one_second(1, 0); - -std::shared_ptr makeLateralNode() -{ - // Pass default parameter file to the node - const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_node"); - rclcpp::NodeOptions node_options; - node_options.arguments( - {"--ros-args", "--params-file", share_dir + "/param/lateral_controller_defaults.param.yaml", - "--params-file", share_dir + "/param/test_vehicle_info.param.yaml", "--params-file", - share_dir + "/param/test_nearest_search.param.yaml"}); - std::shared_ptr node = std::make_shared(node_options); - - // Enable all logging in the node - auto ret = - rcutils_logging_set_logger_level(node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); - if (ret != RCUTILS_RET_OK) { - std::cout << "Failed to set logging severity to DEBUG\n"; - } - return node; -} - -TEST_F(FakeNodeFixture, no_input) -{ - // Data to test - LateralCommand::SharedPtr cmd_msg; - bool received_lateral_command = false; - // Node - std::shared_ptr node = makeLateralNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("lateral_controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("lateral_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("lateral_controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "lateral_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) { - cmd_msg = msg; - received_lateral_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // No published data: expect a stopped command - test_utils::waitForMessage( - node, this, received_lateral_command, std::chrono::seconds{1LL}, false); - ASSERT_FALSE(received_lateral_command); -} - -TEST_F(FakeNodeFixture, empty_trajectory) -{ - // Data to test - LateralCommand::SharedPtr cmd_msg; - bool received_lateral_command = false; - // Node - std::shared_ptr node = makeLateralNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("lateral_controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("lateral_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("lateral_controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "lateral_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) { - cmd_msg = msg; - received_lateral_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - - // Spin for transform to be published - test_utils::spinWhile(node); - - // Empty trajectory: expect a stopped command - Trajectory traj_msg; - traj_msg.header.stamp = node->now(); - traj_msg.header.frame_id = "map"; - VehicleOdometry odom_msg; - SteeringReport steer_msg; - traj_msg.header.stamp = node->now(); - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 0.0; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - traj_pub->publish(traj_msg); - odom_pub->publish(odom_msg); - steer_pub->publish(steer_msg); - - test_utils::waitForMessage( - node, this, received_lateral_command, std::chrono::seconds{1LL}, false); - ASSERT_FALSE(received_lateral_command); -} - -TEST_F(FakeNodeFixture, straight_trajectory) -{ - // Data to test - LateralCommand::SharedPtr cmd_msg; - bool received_lateral_command = false; - // Node - std::shared_ptr node = makeLateralNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("lateral_controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("lateral_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("lateral_controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "lateral_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) { - cmd_msg = msg; - received_lateral_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - - // Spin for transform to be published - test_utils::spinWhile(node); - - // Straight trajectory: expect no steering - received_lateral_command = false; - Trajectory traj_msg; - traj_msg.header.stamp = node->now(); - traj_msg.header.frame_id = "map"; - VehicleOdometry odom_msg; - SteeringReport steer_msg; - TrajectoryPoint p; - traj_msg.header.stamp = node->now(); - p.pose.position.x = -1.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 0.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 1.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 2.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - traj_pub->publish(traj_msg); - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 1.0; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - odom_pub->publish(odom_msg); - steer_pub->publish(steer_msg); - - test_utils::waitForMessage(node, this, received_lateral_command); - ASSERT_TRUE(received_lateral_command); - EXPECT_EQ(cmd_msg->steering_tire_angle, 0.0f); - EXPECT_EQ(cmd_msg->steering_tire_rotation_rate, 0.0f); - EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); -} - -TEST_F(FakeNodeFixture, right_turn) -{ - // Data to test - LateralCommand::SharedPtr cmd_msg; - bool received_lateral_command = false; - // Node - std::shared_ptr node = makeLateralNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("lateral_controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("lateral_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("lateral_controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "lateral_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) { - cmd_msg = msg; - received_lateral_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - - // Spin for transform to be published - test_utils::spinWhile(node); - - // Right turning trajectory: expect right steering - received_lateral_command = false; - Trajectory traj_msg; - traj_msg.header.stamp = node->now(); - traj_msg.header.frame_id = "map"; - VehicleOdometry odom_msg; - SteeringReport steer_msg; - TrajectoryPoint p; - traj_msg.points.clear(); - p.pose.position.x = -1.0; - p.pose.position.y = -1.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 0.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 1.0; - p.pose.position.y = -1.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 2.0; - p.pose.position.y = -2.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - traj_pub->publish(traj_msg); - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 1.0; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - odom_pub->publish(odom_msg); - steer_pub->publish(steer_msg); - - test_utils::waitForMessage(node, this, received_lateral_command); - ASSERT_TRUE(received_lateral_command); - EXPECT_LT(cmd_msg->steering_tire_angle, 0.0f); - EXPECT_LT(cmd_msg->steering_tire_rotation_rate, 0.0f); - EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); -} - -TEST_F(FakeNodeFixture, left_turn) -{ - // Data to test - LateralCommand::SharedPtr cmd_msg; - bool received_lateral_command = false; - // Node - std::shared_ptr node = makeLateralNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("lateral_controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("lateral_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("lateral_controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "lateral_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) { - cmd_msg = msg; - received_lateral_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - - // Spin for transform to be published - test_utils::spinWhile(node); - - // Left turning trajectory: expect left steering - received_lateral_command = false; - Trajectory traj_msg; - traj_msg.header.stamp = node->now(); - traj_msg.header.frame_id = "map"; - VehicleOdometry odom_msg; - SteeringReport steer_msg; - TrajectoryPoint p; - traj_msg.points.clear(); - p.pose.position.x = -1.0; - p.pose.position.y = 1.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 0.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 1.0; - p.pose.position.y = 1.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 2.0; - p.pose.position.y = 2.0; - p.longitudinal_velocity_mps = 1.0f; - traj_msg.points.push_back(p); - traj_pub->publish(traj_msg); - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 1.0; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = 0.0; - odom_pub->publish(odom_msg); - steer_pub->publish(steer_msg); - - test_utils::waitForMessage(node, this, received_lateral_command); - ASSERT_TRUE(received_lateral_command); - EXPECT_GT(cmd_msg->steering_tire_angle, 0.0f); - EXPECT_GT(cmd_msg->steering_tire_rotation_rate, 0.0f); - EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); -} - -TEST_F(FakeNodeFixture, stopped) -{ - // Data to test - LateralCommand::SharedPtr cmd_msg; - bool received_lateral_command = false; - // Node - std::shared_ptr node = makeLateralNode(); - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("lateral_controller/input/reference_trajectory"); - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("lateral_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr steer_pub = - this->create_publisher("lateral_controller/input/current_steering"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "lateral_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_lateral_command](const LateralCommand::SharedPtr msg) { - cmd_msg = msg; - received_lateral_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - - // Spin for transform to be published - test_utils::spinWhile(node); - - // Straight trajectory: expect no steering - received_lateral_command = false; - Trajectory traj_msg; - traj_msg.header.stamp = node->now(); - traj_msg.header.frame_id = "map"; - VehicleOdometry odom_msg; - SteeringReport steer_msg; - TrajectoryPoint p; - traj_msg.header.stamp = node->now(); - p.pose.position.x = -1.0; - p.pose.position.y = 0.0; - // Set a 0 current velocity and 0 target velocity -> stopped state - p.longitudinal_velocity_mps = 0.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 0.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 0.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 1.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 0.0f; - traj_msg.points.push_back(p); - p.pose.position.x = 2.0; - p.pose.position.y = 0.0; - p.longitudinal_velocity_mps = 0.0f; - traj_msg.points.push_back(p); - traj_pub->publish(traj_msg); - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 0.0; - steer_msg.stamp = node->now(); - steer_msg.steering_tire_angle = -0.5; - odom_pub->publish(odom_msg); - steer_pub->publish(steer_msg); - - test_utils::waitForMessage(node, this, received_lateral_command); - ASSERT_TRUE(received_lateral_command); - EXPECT_EQ(cmd_msg->steering_tire_angle, steer_msg.steering_tire_angle); - EXPECT_EQ(cmd_msg->steering_tire_rotation_rate, 0.0f); - EXPECT_GT(rclcpp::Time(cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); -} diff --git a/control/trajectory_follower_node/test/test_longitudinal_controller_node.cpp b/control/trajectory_follower_node/test/test_longitudinal_controller_node.cpp deleted file mode 100644 index badafd5f2fd15..0000000000000 --- a/control/trajectory_follower_node/test/test_longitudinal_controller_node.cpp +++ /dev/null @@ -1,472 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ament_index_cpp/get_package_share_directory.hpp" -#include "fake_test_node/fake_test_node.hpp" -#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/time.hpp" -#include "trajectory_follower_test_utils.hpp" - -#include - -#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" - -#include - -using LongitudinalController = - autoware::motion::control::trajectory_follower_node::LongitudinalController; -using LongitudinalCommand = autoware_auto_control_msgs::msg::LongitudinalCommand; -using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; -using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; -using VehicleOdometry = nav_msgs::msg::Odometry; - -using FakeNodeFixture = autoware::tools::testing::FakeTestNode; - -std::shared_ptr makeLongitudinalNode() -{ - // Pass default parameter file to the node - const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_node"); - rclcpp::NodeOptions node_options; - node_options.arguments( - {"--ros-args", "--params-file", - share_dir + "/param/longitudinal_controller_defaults.param.yaml", "--params-file", - share_dir + "/param/test_vehicle_info.param.yaml", "--params-file", - share_dir + "/param/test_nearest_search.param.yaml"}); - std::shared_ptr node = - std::make_shared(node_options); - - // Enable all logging in the node - auto ret = - rcutils_logging_set_logger_level(node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); - if (ret != RCUTILS_RET_OK) { - std::cout << "Failed to set logging severity to DEBUG\n"; - } - return node; -} - -TEST_F(FakeNodeFixture, longitudinal_keep_velocity) -{ - // Data to test - LongitudinalCommand::SharedPtr cmd_msg; - bool received_longitudinal_command = false; - - // Node - std::shared_ptr node = makeLongitudinalNode(); - - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("longitudinal_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("longitudinal_controller/input/current_trajectory"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "longitudinal_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { - cmd_msg = msg; - received_longitudinal_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - /// Already running at target vel + Non stopping trajectory -> no change in velocity - // Publish velocity - VehicleOdometry odom_msg; - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 1.0; - odom_pub->publish(odom_msg); - // the node needs to receive two velocity msg - rclcpp::spin_some(node); - rclcpp::spin_some(this->get_fake_node()); - odom_msg.header.stamp = node->now(); - odom_pub->publish(odom_msg); - // Publish non stopping trajectory - Trajectory traj; - traj.header.stamp = node->now(); - traj.header.frame_id = "map"; - TrajectoryPoint point; - point.pose.position.x = 0.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - point.pose.position.x = 50.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - point.pose.position.x = 100.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_longitudinal_command); - - ASSERT_TRUE(received_longitudinal_command); - EXPECT_DOUBLE_EQ(cmd_msg->speed, 1.0); - EXPECT_DOUBLE_EQ(cmd_msg->acceleration, 0.0); - - // Generate another control message - received_longitudinal_command = false; - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_longitudinal_command); - ASSERT_TRUE(received_longitudinal_command); - EXPECT_DOUBLE_EQ(cmd_msg->speed, 1.0); - EXPECT_DOUBLE_EQ(cmd_msg->acceleration, 0.0); -} - -TEST_F(FakeNodeFixture, longitudinal_slow_down) -{ - // Data to test - LongitudinalCommand::SharedPtr cmd_msg; - bool received_longitudinal_command = false; - - // Node - std::shared_ptr node = makeLongitudinalNode(); - - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("longitudinal_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("longitudinal_controller/input/current_trajectory"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "longitudinal_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { - cmd_msg = msg; - received_longitudinal_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - /// Already running at target vel + Non stopping trajectory -> no change in velocity - // Publish velocity - VehicleOdometry odom_msg; - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 1.0; - odom_pub->publish(odom_msg); - // the node needs to receive two velocity msg - rclcpp::spin_some(node); - rclcpp::spin_some(this->get_fake_node()); - odom_msg.header.stamp = node->now(); - odom_pub->publish(odom_msg); - // Publish non stopping trajectory - Trajectory traj; - traj.header.stamp = node->now(); - traj.header.frame_id = "map"; - TrajectoryPoint point; - point.pose.position.x = 0.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 0.5; - traj.points.push_back(point); - point.pose.position.x = 50.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 0.5; - traj.points.push_back(point); - point.pose.position.x = 100.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 0.5; - traj.points.push_back(point); - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_longitudinal_command); - - ASSERT_TRUE(received_longitudinal_command); - EXPECT_LT(cmd_msg->speed, static_cast(odom_msg.twist.twist.linear.x)); - EXPECT_LT(cmd_msg->acceleration, 0.0f); - - // Generate another control message - received_longitudinal_command = false; - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_longitudinal_command); - ASSERT_TRUE(received_longitudinal_command); - EXPECT_LT(cmd_msg->speed, static_cast(odom_msg.twist.twist.linear.x)); - EXPECT_LT(cmd_msg->acceleration, 0.0f); -} - -TEST_F(FakeNodeFixture, longitudinal_accelerate) -{ - // Data to test - LongitudinalCommand::SharedPtr cmd_msg; - bool received_longitudinal_command = false; - - // Node - std::shared_ptr node = makeLongitudinalNode(); - - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("longitudinal_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("longitudinal_controller/input/current_trajectory"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "longitudinal_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { - cmd_msg = msg; - received_longitudinal_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - /// Below target vel + Non stopping trajectory -> accelerate - // Publish velocity - VehicleOdometry odom_msg; - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 0.5; - odom_pub->publish(odom_msg); - // the node needs to receive two velocity msg - rclcpp::spin_some(node); - rclcpp::spin_some(this->get_fake_node()); - odom_msg.header.stamp = node->now(); - odom_pub->publish(odom_msg); - // Publish non stopping trajectory - Trajectory traj; - traj.header.stamp = node->now(); - traj.header.frame_id = "map"; - TrajectoryPoint point; - point.pose.position.x = 0.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - point.pose.position.x = 50.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - point.pose.position.x = 100.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_longitudinal_command); - - ASSERT_TRUE(received_longitudinal_command); - EXPECT_GT(cmd_msg->speed, static_cast(odom_msg.twist.twist.linear.x)); - EXPECT_GT(cmd_msg->acceleration, 0.0f); - - // Generate another control message - received_longitudinal_command = false; - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_longitudinal_command); - ASSERT_TRUE(received_longitudinal_command); - EXPECT_GT(cmd_msg->speed, static_cast(odom_msg.twist.twist.linear.x)); - EXPECT_GT(cmd_msg->acceleration, 0.0f); -} - -TEST_F(FakeNodeFixture, longitudinal_stopped) -{ - // Data to test - LongitudinalCommand::SharedPtr cmd_msg; - bool received_longitudinal_command = false; - - // Node - std::shared_ptr node = makeLongitudinalNode(); - - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("longitudinal_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("longitudinal_controller/input/current_trajectory"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "longitudinal_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { - cmd_msg = msg; - received_longitudinal_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - /// Below target vel + Non stopping trajectory -> accelerate - // Publish velocity - VehicleOdometry odom_msg; - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 0.0; - odom_pub->publish(odom_msg); - // the node needs to receive two velocity msg - rclcpp::spin_some(node); - rclcpp::spin_some(this->get_fake_node()); - odom_msg.header.stamp = node->now(); - odom_pub->publish(odom_msg); - // Publish stopping trajectory - Trajectory traj; - traj.header.stamp = node->now(); - traj.header.frame_id = "map"; - TrajectoryPoint point; - point.pose.position.x = 0.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 0.0; - traj.points.push_back(point); - point.pose.position.x = 50.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 0.0; - traj.points.push_back(point); - point.pose.position.x = 100.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 0.0; - traj.points.push_back(point); - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_longitudinal_command); - - ASSERT_TRUE(received_longitudinal_command); - EXPECT_DOUBLE_EQ(cmd_msg->speed, 0.0f); - EXPECT_LT(cmd_msg->acceleration, 0.0f); // when stopped negative acceleration to brake -} - -TEST_F(FakeNodeFixture, longitudinal_reverse) -{ - // Data to test - LongitudinalCommand::SharedPtr cmd_msg; - bool received_longitudinal_command = false; - - // Node - std::shared_ptr node = makeLongitudinalNode(); - - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("longitudinal_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("longitudinal_controller/input/current_trajectory"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "longitudinal_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { - cmd_msg = msg; - received_longitudinal_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - /// Below target vel + Non stopping trajectory -> accelerate - // Publish velocity - VehicleOdometry odom_msg; - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 0.0; - odom_pub->publish(odom_msg); - // the node needs to receive two velocity msg - rclcpp::spin_some(node); - rclcpp::spin_some(this->get_fake_node()); - odom_msg.header.stamp = node->now(); - odom_pub->publish(odom_msg); - // Publish reverse - Trajectory traj; - traj.header.stamp = node->now(); - traj.header.frame_id = "map"; - TrajectoryPoint point; - point.pose.position.x = 0.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = -1.0; - traj.points.push_back(point); - point.pose.position.x = 50.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = -1.0; - traj.points.push_back(point); - point.pose.position.x = 100.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = -1.0; - traj.points.push_back(point); - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_longitudinal_command); - - ASSERT_TRUE(received_longitudinal_command); - EXPECT_LT(cmd_msg->speed, 0.0f); - EXPECT_GT(cmd_msg->acceleration, 0.0f); -} - -TEST_F(FakeNodeFixture, longitudinal_emergency) -{ - // Data to test - LongitudinalCommand::SharedPtr cmd_msg; - bool received_longitudinal_command = false; - - // Node - std::shared_ptr node = makeLongitudinalNode(); - - // Publisher/Subscribers - rclcpp::Publisher::SharedPtr odom_pub = - this->create_publisher("longitudinal_controller/input/current_odometry"); - rclcpp::Publisher::SharedPtr traj_pub = - this->create_publisher("longitudinal_controller/input/current_trajectory"); - rclcpp::Subscription::SharedPtr cmd_sub = - this->create_subscription( - "longitudinal_controller/output/control_cmd", *this->get_fake_node(), - [&cmd_msg, &received_longitudinal_command](const LongitudinalCommand::SharedPtr msg) { - cmd_msg = msg; - received_longitudinal_command = true; - }); - std::shared_ptr br = - std::make_shared(this->get_fake_node()); - - // Dummy transform: ego is at (0.0, 0.0) in map frame - geometry_msgs::msg::TransformStamped transform = test_utils::getDummyTransform(); - transform.header.stamp = node->now(); - br->sendTransform(transform); - /// Below target vel + Non stopping trajectory -> accelerate - // Publish velocity - VehicleOdometry odom_msg; - odom_msg.header.stamp = node->now(); - odom_msg.twist.twist.linear.x = 0.0; - odom_pub->publish(odom_msg); - // the node needs to receive two velocity msg - rclcpp::spin_some(node); - rclcpp::spin_some(this->get_fake_node()); - odom_msg.header.stamp = node->now(); - odom_pub->publish(odom_msg); - // Publish trajectory starting away from the current ego pose - Trajectory traj; - traj.header.stamp = node->now(); - traj.header.frame_id = "map"; - TrajectoryPoint point; - point.pose.position.x = 10.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - point.pose.position.x = 50.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - point.pose.position.x = 100.0; - point.pose.position.y = 0.0; - point.longitudinal_velocity_mps = 1.0; - traj.points.push_back(point); - traj_pub->publish(traj); - test_utils::waitForMessage(node, this, received_longitudinal_command); - - ASSERT_TRUE(received_longitudinal_command); - // Emergencies (e.g., far from trajectory) produces braking command (0 vel, negative accel) - EXPECT_DOUBLE_EQ(cmd_msg->speed, 0.0f); - EXPECT_LT(cmd_msg->acceleration, 0.0f); -} diff --git a/control/vehicle_cmd_gate/package.xml b/control/vehicle_cmd_gate/package.xml index deb988d61738f..22ae9da6d222e 100644 --- a/control/vehicle_cmd_gate/package.xml +++ b/control/vehicle_cmd_gate/package.xml @@ -28,6 +28,7 @@ rclcpp_components std_srvs tier4_api_utils + tier4_autoware_utils tier4_control_msgs tier4_debug_msgs tier4_external_api_msgs diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index aec2ede53d9e7..27592b9405fc5 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -242,6 +242,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) this, get_clock(), period_ns, std::bind(&VehicleCmdGate::publishStatus, this)); logger_configure_ = std::make_unique(this); + + published_time_publisher_ = std::make_unique(this); } bool VehicleCmdGate::isHeartbeatTimeout( @@ -456,6 +458,8 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) // Publish commands vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency); control_cmd_pub_->publish(filtered_commands.control); + published_time_publisher_->publish_if_subscribed( + control_cmd_pub_, filtered_commands.control.stamp); adapi_pause_->publish(); moderate_stop_interface_->publish(); diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index b79c58d120443..1907cf123632f 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -248,6 +249,8 @@ class VehicleCmdGate : public rclcpp::Node void publishMarkers(const IsFilterActivated & filter_activated); std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; }; } // namespace vehicle_cmd_gate diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp index dd6756a17f194..20397448bf21a 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp @@ -19,6 +19,7 @@ #include "perception_online_evaluator/metrics/metric.hpp" #include "perception_online_evaluator/parameters.hpp" #include "perception_online_evaluator/stat.hpp" +#include "perception_online_evaluator/utils/objects_filtering.hpp" #include @@ -118,9 +119,9 @@ class MetricsCalculator void deleteOldObjects(const rclcpp::Time stamp); // Calculate metrics - MetricStatMap calcLateralDeviationMetrics(const PredictedObjects & objects) const; - MetricStatMap calcYawDeviationMetrics(const PredictedObjects & objects) const; - MetricStatMap calcPredictedPathDeviationMetrics(const PredictedObjects & objects) const; + MetricStatMap calcLateralDeviationMetrics(const ClassObjectsMap & class_objects_map) const; + MetricStatMap calcYawDeviationMetrics(const ClassObjectsMap & class_objects_map) const; + MetricStatMap calcPredictedPathDeviationMetrics(const ClassObjectsMap & class_objects_map) const; Stat calcPredictedPathDeviationMetrics( const PredictedObjects & objects, const double time_horizon) const; diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp index 5d4238f45dec9..8a118a14d7368 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp @@ -37,6 +37,8 @@ using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; +using ClassObjectsMap = std::unordered_map; + std::uint8_t getHighestProbLabel(const std::vector & classification); /** @@ -105,6 +107,14 @@ void filterObjects(PredictedObjects & objects, Func filter) filterObjects(objects, removed_objects, filter); } +/** + * @brief Separates the provided objects based on their classification. + * + * @param objects The predicted objects to be separated. + * @return A map of objects separated by their classification. + */ +ClassObjectsMap separateObjectsByClass(const PredictedObjects & objects); + /** * @brief Filters the provided objects based on their classification. * diff --git a/evaluator/perception_online_evaluator/package.xml b/evaluator/perception_online_evaluator/package.xml index bc0f7ef82049b..18dde41c99566 100644 --- a/evaluator/perception_online_evaluator/package.xml +++ b/evaluator/perception_online_evaluator/package.xml @@ -21,9 +21,11 @@ diagnostic_msgs eigen geometry_msgs + lanelet2_extension libgoogle-glog-dev motion_utils nav_msgs + object_recognition_utils pluginlib rclcpp rclcpp_components diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index bec6d84fcd7dd..3afcdc0cd3185 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -15,13 +15,15 @@ #include "perception_online_evaluator/metrics_calculator.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "perception_online_evaluator/utils/objects_filtering.hpp" +#include "object_recognition_utils/object_recognition_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include namespace perception_diagnostics { +using object_recognition_utils::convertLabelToString; + std::optional MetricsCalculator::calculate(const Metric & metric) const { if (object_map_.empty()) { @@ -35,14 +37,14 @@ std::optional MetricsCalculator::calculate(const Metric & metric) return {}; } const auto target_objects = getObjectsByStamp(target_stamp); - + const ClassObjectsMap class_objects_map = separateObjectsByClass(target_objects); switch (metric) { case Metric::lateral_deviation: - return calcLateralDeviationMetrics(target_objects); + return calcLateralDeviationMetrics(class_objects_map); case Metric::yaw_deviation: - return calcYawDeviationMetrics(target_objects); + return calcYawDeviationMetrics(class_objects_map); case Metric::predicted_path_deviation: - return calcPredictedPathDeviationMetrics(target_objects); + return calcPredictedPathDeviationMetrics(class_objects_map); default: return {}; } @@ -155,65 +157,70 @@ PredictedObjects MetricsCalculator::getObjectsByStamp(const rclcpp::Time stamp) return objects; } -MetricStatMap MetricsCalculator::calcLateralDeviationMetrics(const PredictedObjects & objects) const +MetricStatMap MetricsCalculator::calcLateralDeviationMetrics( + const ClassObjectsMap & class_objects_map) const { - Stat stat{}; - const auto stamp = rclcpp::Time(objects.header.stamp); - - for (const auto & object : objects.objects) { - const auto uuid = tier4_autoware_utils::toHexString(object.object_id); - if (!hasPassedTime(uuid, stamp)) { - continue; - } - const auto object_pose = object.kinematics.initial_pose_with_covariance.pose; - const auto history_path = history_path_map_.at(uuid).second; - if (history_path.empty()) { - continue; + MetricStatMap metric_stat_map{}; + for (const auto & [label, objects] : class_objects_map) { + Stat stat{}; + const auto stamp = rclcpp::Time(objects.header.stamp); + for (const auto & object : objects.objects) { + const auto uuid = tier4_autoware_utils::toHexString(object.object_id); + if (!hasPassedTime(uuid, stamp)) { + continue; + } + const auto object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto history_path = history_path_map_.at(uuid).second; + if (history_path.empty()) { + continue; + } + stat.add(metrics::calcLateralDeviation(history_path, object_pose)); } - stat.add(metrics::calcLateralDeviation(history_path, object_pose)); + metric_stat_map["lateral_deviation_" + convertLabelToString(label)] = stat; } - - MetricStatMap metric_stat_map; - metric_stat_map["lateral_deviation"] = stat; return metric_stat_map; } -MetricStatMap MetricsCalculator::calcYawDeviationMetrics(const PredictedObjects & objects) const +MetricStatMap MetricsCalculator::calcYawDeviationMetrics( + const ClassObjectsMap & class_objects_map) const { - Stat stat{}; - const auto stamp = rclcpp::Time(objects.header.stamp); - for (const auto & object : objects.objects) { - const auto uuid = tier4_autoware_utils::toHexString(object.object_id); - if (!hasPassedTime(uuid, stamp)) { - continue; - } - const auto object_pose = object.kinematics.initial_pose_with_covariance.pose; - const auto history_path = history_path_map_.at(uuid).second; - if (history_path.empty()) { - continue; + MetricStatMap metric_stat_map{}; + for (const auto & [label, objects] : class_objects_map) { + Stat stat{}; + const auto stamp = rclcpp::Time(objects.header.stamp); + for (const auto & object : objects.objects) { + const auto uuid = tier4_autoware_utils::toHexString(object.object_id); + if (!hasPassedTime(uuid, stamp)) { + continue; + } + const auto object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto history_path = history_path_map_.at(uuid).second; + if (history_path.empty()) { + continue; + } + stat.add(metrics::calcYawDeviation(history_path, object_pose)); } - stat.add(metrics::calcYawDeviation(history_path, object_pose)); + metric_stat_map["yaw_deviation_" + convertLabelToString(label)] = stat; } - - MetricStatMap metric_stat_map; - metric_stat_map["yaw_deviation"] = stat; return metric_stat_map; } MetricStatMap MetricsCalculator::calcPredictedPathDeviationMetrics( - const PredictedObjects & objects) const + const ClassObjectsMap & class_objects_map) const { const auto time_horizons = parameters_->prediction_time_horizons; - MetricStatMap metric_stat_map; - for (const double time_horizon : time_horizons) { - const auto stat = calcPredictedPathDeviationMetrics(objects, time_horizon); - std::ostringstream stream; - stream << std::fixed << std::setprecision(2) << time_horizon; - std::string time_horizon_str = stream.str(); - metric_stat_map["predicted_path_deviation_" + time_horizon_str] = stat; + MetricStatMap metric_stat_map{}; + for (const auto & [label, objects] : class_objects_map) { + for (const double time_horizon : time_horizons) { + const auto stat = calcPredictedPathDeviationMetrics(objects, time_horizon); + std::ostringstream stream; + stream << std::fixed << std::setprecision(2) << time_horizon; + std::string time_horizon_str = stream.str(); + metric_stat_map + ["predicted_path_deviation_" + convertLabelToString(label) + "_" + time_horizon_str] = stat; + } } - return metric_stat_map; } diff --git a/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp b/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp index 97daee36175f3..e39ba348ee42b 100644 --- a/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp +++ b/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp @@ -83,6 +83,17 @@ bool isTargetObjectType( (t == ObjectClassification::PEDESTRIAN && target_object_types.check_pedestrian)); } +ClassObjectsMap separateObjectsByClass(const PredictedObjects & objects) +{ + ClassObjectsMap separated_objects; + for (const auto & object : objects.objects) { + const auto label = getHighestProbLabel(object.classification); + separated_objects[label].objects.push_back(object); + separated_objects[label].header = objects.header; + } + return separated_objects; +} + void filterObjectsByClass( PredictedObjects & objects, const ObjectTypesToCheck & target_object_types) { diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp index 095fb130426f0..16c46d0eecd92 100644 --- a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -79,7 +79,9 @@ class EvalTest : public ::testing::Test marker_sub_ = rclcpp::create_subscription( eval_node, "perception_online_evaluator/markers", 10, - [this]([[maybe_unused]] const MarkerArray::SharedPtr msg) { has_received_marker_ = true; }); + [this]([[maybe_unused]] const MarkerArray::ConstSharedPtr msg) { + has_received_marker_ = true; + }); uuid_ = generateUUID(); } @@ -105,21 +107,20 @@ class EvalTest : public ::testing::Test [=](const DiagnosticArray::ConstSharedPtr msg) { const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric); if (it != msg->status.end()) { - std::cerr << it->values[0].key << " " << it->values[0].value << " " << it->values[1].key - << " " << it->values[1].value << " " << it->values[2].key << " " - << it->values[2].value << std::endl; metric_value_ = boost::lexical_cast(it->values[2].value); metric_updated_ = true; } }); } - PredictedObject makePredictedObject(const std::vector> & predicted_path) + PredictedObject makePredictedObject( + const std::vector> & predicted_path, + const uint8_t label = ObjectClassification::CAR) { PredictedObject object; object.object_id = uuid_; ObjectClassification classification; - classification.label = ObjectClassification::CAR; + classification.label = label; classification.probability = 1.0; object.classification = {classification}; @@ -153,32 +154,35 @@ class EvalTest : public ::testing::Test } PredictedObjects makePredictedObjects( - const std::vector> & predicted_path) + const std::vector> & predicted_path, + const uint8_t label = ObjectClassification::CAR) { PredictedObjects objects; - objects.objects.push_back(makePredictedObject(predicted_path)); + objects.objects.push_back(makePredictedObject(predicted_path, label)); objects.header.stamp = rclcpp::Time(0); return objects; } - PredictedObjects makeStraightPredictedObjects(const double time) + PredictedObjects makeStraightPredictedObjects( + const double time, const uint8_t label = ObjectClassification::CAR) { std::vector> predicted_path; for (size_t i = 0; i <= time_horizon_ / time_step_; i++) { predicted_path.push_back({velocity_ * (time + i * time_step_), 0.0}); } - auto objects = makePredictedObjects(predicted_path); + auto objects = makePredictedObjects(predicted_path, label); objects.header.stamp = rclcpp::Time(0) + rclcpp::Duration::from_seconds(time); return objects; } - PredictedObjects makeDeviatedStraightPredictedObjects(const double time, const double deviation) + PredictedObjects makeDeviatedStraightPredictedObjects( + const double time, const double deviation, const uint8_t label = ObjectClassification::CAR) { std::vector> predicted_path; for (size_t i = 0; i <= time_horizon_ / time_step_; i++) { predicted_path.push_back({velocity_ * (time + i * time_step_), deviation}); } - auto objects = makePredictedObjects(predicted_path); + auto objects = makePredictedObjects(predicted_path, label); objects.header.stamp = rclcpp::Time(0) + rclcpp::Duration::from_seconds(time); return objects; } @@ -255,7 +259,7 @@ class EvalTest : public ::testing::Test TEST_F(EvalTest, testLateralDeviation_deviation0) { waitForDummyNode(); - setTargetMetric("lateral_deviation"); + setTargetMetric("lateral_deviation_CAR"); const double deviation = 0.0; for (double time = 0; time < time_delay_; time += time_step_) { @@ -270,7 +274,7 @@ TEST_F(EvalTest, testLateralDeviation_deviation0) TEST_F(EvalTest, testLateralDeviation_deviation1) { waitForDummyNode(); - setTargetMetric("lateral_deviation"); + setTargetMetric("lateral_deviation_CAR"); const double deviation = 1.0; for (double time = 0; time < time_delay_; time += time_step_) { @@ -285,7 +289,7 @@ TEST_F(EvalTest, testLateralDeviation_deviation1) TEST_F(EvalTest, testLateralDeviation_oscillation) { waitForDummyNode(); - setTargetMetric("lateral_deviation"); + setTargetMetric("lateral_deviation_CAR"); const double deviation = 1.0; double sign = 1.0; @@ -307,7 +311,7 @@ TEST_F(EvalTest, testLateralDeviation_oscillation) TEST_F(EvalTest, testLateralDeviation_distortion) { waitForDummyNode(); - setTargetMetric("lateral_deviation"); + setTargetMetric("lateral_deviation_CAR"); const double deviation = 1.0; for (double time = 0; time < time_delay_ * 2; time += time_step_) { @@ -325,6 +329,23 @@ TEST_F(EvalTest, testLateralDeviation_distortion) const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation); EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), deviation, epsilon); } + +TEST_F(EvalTest, testLateralDeviation_deviation0_PEDESTRIAN) +{ + waitForDummyNode(); + setTargetMetric("lateral_deviation_PEDESTRIAN"); + + const double deviation = 0.0; + for (double time = 0; time < time_delay_; time += time_step_) { + const auto objects = + makeDeviatedStraightPredictedObjects(time, deviation, ObjectClassification::PEDESTRIAN); + publishObjects(objects); + } + + const auto last_objects = + makeDeviatedStraightPredictedObjects(time_delay_, deviation, ObjectClassification::PEDESTRIAN); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} // ========================================================================================== // ========================================================================================== @@ -332,7 +353,7 @@ TEST_F(EvalTest, testLateralDeviation_distortion) TEST_F(EvalTest, testYawDeviation_deviation0) { waitForDummyNode(); - setTargetMetric("yaw_deviation"); + setTargetMetric("yaw_deviation_CAR"); const double deviation = 0.0; for (double time = 0; time < time_delay_; time += time_step_) { @@ -347,7 +368,7 @@ TEST_F(EvalTest, testYawDeviation_deviation0) TEST_F(EvalTest, testYawDeviation_deviation1) { waitForDummyNode(); - setTargetMetric("yaw_deviation"); + setTargetMetric("yaw_deviation_CAR"); const double deviation = 1.0; for (double time = 0; time < time_delay_; time += time_step_) { @@ -362,7 +383,7 @@ TEST_F(EvalTest, testYawDeviation_deviation1) TEST_F(EvalTest, testYawDeviation_oscillation) { waitForDummyNode(); - setTargetMetric("yaw_deviation"); + setTargetMetric("yaw_deviation_CAR"); const double deviation = 1.0; double sign = 1.0; @@ -384,7 +405,7 @@ TEST_F(EvalTest, testYawDeviation_oscillation) TEST_F(EvalTest, testYawDeviation_distortion) { waitForDummyNode(); - setTargetMetric("yaw_deviation"); + setTargetMetric("yaw_deviation_CAR"); const double deviation = 1.0; for (double time = 0; time < time_delay_ * 2; time += time_step_) { @@ -406,7 +427,7 @@ TEST_F(EvalTest, testYawDeviation_distortion) TEST_F(EvalTest, testYawDeviation_oscillation_rotate) { waitForDummyNode(); - setTargetMetric("yaw_deviation"); + setTargetMetric("yaw_deviation_CAR"); const double deviation = 1.0; const double yaw = M_PI / 4; @@ -431,7 +452,7 @@ TEST_F(EvalTest, testYawDeviation_oscillation_rotate) TEST_F(EvalTest, testYawDeviation_distortion_rotate) { waitForDummyNode(); - setTargetMetric("yaw_deviation"); + setTargetMetric("yaw_deviation_CAR"); const double deviation = 1.0; const double yaw = M_PI / 4; @@ -454,13 +475,30 @@ TEST_F(EvalTest, testYawDeviation_distortion_rotate) EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), yaw, epsilon); } +TEST_F(EvalTest, testYawDeviation_deviation0_PEDESTRIAN) +{ + waitForDummyNode(); + setTargetMetric("yaw_deviation_PEDESTRIAN"); + + const double deviation = 0.0; + for (double time = 0; time < time_delay_; time += time_step_) { + const auto objects = + makeDeviatedStraightPredictedObjects(time, deviation, ObjectClassification::PEDESTRIAN); + publishObjects(objects); + } + + const auto last_objects = + makeDeviatedStraightPredictedObjects(time_delay_, deviation, ObjectClassification::PEDESTRIAN); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + // ========================================================================================== // predicted path deviation{ TEST_F(EvalTest, testPredictedPathDeviation_deviation0) { waitForDummyNode(); - setTargetMetric("predicted_path_deviation_5.00"); + setTargetMetric("predicted_path_deviation_CAR_5.00"); const auto init_objects = makeStraightPredictedObjects(0); publishObjects(init_objects); @@ -481,7 +519,7 @@ TEST_F(EvalTest, testPredictedPathDeviation_deviation1) { waitForDummyNode(); - setTargetMetric("predicted_path_deviation_5.00"); + setTargetMetric("predicted_path_deviation_CAR_5.00"); const auto init_objects = makeStraightPredictedObjects(0); publishObjects(init_objects); @@ -502,7 +540,7 @@ TEST_F(EvalTest, testPredictedPathDeviation_deviation2) { waitForDummyNode(); - setTargetMetric("predicted_path_deviation_5.00"); + setTargetMetric("predicted_path_deviation_CAR_5.00"); const auto init_objects = makeStraightPredictedObjects(0); publishObjects(init_objects); @@ -518,4 +556,27 @@ TEST_F(EvalTest, testPredictedPathDeviation_deviation2) const double mean_deviation = deviation * (num_points - 1) / num_points; EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), mean_deviation, epsilon); } + +TEST_F(EvalTest, testPredictedPathDeviation_deviation0_PEDESTRIAN) +{ + waitForDummyNode(); + + setTargetMetric("predicted_path_deviation_PEDESTRIAN_5.00"); + + const auto init_objects = makeStraightPredictedObjects(0, ObjectClassification::PEDESTRIAN); + publishObjects(init_objects); + + const double deviation = 0.0; + for (double time = time_step_; time < time_delay_; time += time_step_) { + const auto objects = + makeDeviatedStraightPredictedObjects(time, deviation, ObjectClassification::PEDESTRIAN); + publishObjects(objects); + } + const auto last_objects = + makeDeviatedStraightPredictedObjects(time_delay_, deviation, ObjectClassification::PEDESTRIAN); + + const double num_points = time_delay_ / time_step_ + 1; + const double mean_deviation = deviation * (num_points - 1) / num_points; + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), mean_deviation, epsilon); +} // ========================================================================================== diff --git a/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt b/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt index 4c1e8cf58c262..8475b596e6d6b 100644 --- a/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt +++ b/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt @@ -8,11 +8,11 @@ find_package(Qt5 REQUIRED Core Widgets Charts) set(QT_WIDGETS_LIB Qt5::Widgets) set(QT_CHARTS_LIB Qt5::Charts) set(CMAKE_AUTOMOC ON) -set(CMAKE_INCLUDE_CURRENT_DIR ON) add_definitions(-DQT_NO_KEYWORDS) ament_auto_add_library(${PROJECT_NAME} SHARED src/metrics_visualize_panel.cpp + include/metrics_visualize_panel.hpp ) target_link_libraries(${PROJECT_NAME} diff --git a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp b/evaluator/tier4_metrics_rviz_plugin/include/metrics_visualize_panel.hpp similarity index 72% rename from evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp rename to evaluator/tier4_metrics_rviz_plugin/include/metrics_visualize_panel.hpp index 6708ecd7071e9..3d66099988d8b 100644 --- a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp +++ b/evaluator/tier4_metrics_rviz_plugin/include/metrics_visualize_panel.hpp @@ -19,11 +19,13 @@ #ifndef Q_MOC_RUN #include #include +#include #include #include #include #include #include +#include #include #include #endif @@ -33,9 +35,12 @@ #include +#include #include #include #include +#include +#include namespace rviz_plugins { @@ -62,10 +67,10 @@ struct Metric { auto label = new QLabel; label->setAlignment(Qt::AlignCenter); - label->setText("metric_name"); + label->setText(QString::fromStdString(status.name)); labels.emplace("metric_name", label); - header.push_back(QString::fromStdString(status.name)); + header.push_back("metric_name"); } for (const auto & [key, value] : status.values) { @@ -146,6 +151,8 @@ struct Metric QTableWidget * getTable() const { return table; } + std::unordered_map getLabels() const { return labels; } + private: static std::optional getValue(const DiagnosticStatus & status, std::string && key) { @@ -186,19 +193,61 @@ class MetricsVisualizePanel : public rviz_common::Panel void onInitialize() override; private Q_SLOTS: + // Slot functions triggered by UI events + void onTopicChanged(); + void onSpecificMetricChanged(); + void onClearButtonClicked(); + void onTabChanged(); private: + // ROS 2 node and subscriptions for handling metrics data rclcpp::Node::SharedPtr raw_node_; rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Subscription::SharedPtr sub_; + std::unordered_map::SharedPtr> subscriptions_; + + // Topics from which metrics are collected + std::vector topics_ = { + "/diagnostic/planning_evaluator/metrics", "/diagnostic/perception_online_evaluator/metrics"}; + // Timer and metrics message callback void onTimer(); - void onMetrics(const DiagnosticArray::ConstSharedPtr msg); + void onMetrics(const DiagnosticArray::ConstSharedPtr & msg, const std::string & topic_name); + + // Functions to update UI based on selected metrics + void updateViews(); + void updateSelectedMetric(const std::string & metric_name); + // UI components QGridLayout * grid_; + QComboBox * topic_selector_; + QTabWidget * tab_widget_; + + // "Specific Metrics" tab components + QComboBox * specific_metric_selector_; + QChartView * specific_metric_chart_view_; + QTableWidget * specific_metric_table_; + + // Selected metrics data + std::optional> selected_metrics_; + // Cache for received messages by topics + std::unordered_map current_msg_map_; + + // Mapping from topics to metrics widgets (tables and charts) + std::unordered_map< + std::string, std::unordered_map>> + topic_widgets_map_; + + // Synchronization std::mutex mutex_; + + // Stored metrics data std::unordered_map metrics_; + + // Utility functions for managing widget visibility based on topics + void updateWidgetVisibility(const std::string & target_topic, const bool show); + void showCurrentTopicWidgets(); + void hideInactiveTopicWidgets(); }; } // namespace rviz_plugins diff --git a/evaluator/tier4_metrics_rviz_plugin/package.xml b/evaluator/tier4_metrics_rviz_plugin/package.xml index fe84a0ab81760..d06382bc8c539 100644 --- a/evaluator/tier4_metrics_rviz_plugin/package.xml +++ b/evaluator/tier4_metrics_rviz_plugin/package.xml @@ -7,6 +7,8 @@ Satoshi OTA Kyoichi Sugahara Maxime CLEMENT + Kosuke Takeuchi + Fumiya Watanabe Apache License 2.0 ament_cmake_auto diff --git a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp index 79da02b9b65c6..b92a9a7ace53d 100644 --- a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp +++ b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp @@ -29,7 +29,53 @@ namespace rviz_plugins MetricsVisualizePanel::MetricsVisualizePanel(QWidget * parent) : rviz_common::Panel(parent), grid_(new QGridLayout()) { - setLayout(grid_); + // Initialize the main tab widget + tab_widget_ = new QTabWidget(); + + // Create and configure the "All Metrics" tab + QWidget * all_metrics_widget = new QWidget(); + grid_ = new QGridLayout(all_metrics_widget); // Apply grid layout to the widget directly + all_metrics_widget->setLayout(grid_); + + // Add topic selector combobox + topic_selector_ = new QComboBox(); + for (const auto & topic : topics_) { + topic_selector_->addItem(QString::fromStdString(topic)); + } + grid_->addWidget(topic_selector_, 0, 0, 1, -1); // Add topic selector to the grid layout + connect(topic_selector_, SIGNAL(currentIndexChanged(int)), this, SLOT(onTopicChanged())); + + tab_widget_->addTab( + all_metrics_widget, "All Metrics"); // Add "All Metrics" tab to the tab widget + + // Create and configure the "Specific Metrics" tab + QWidget * specific_metrics_widget = new QWidget(); + QVBoxLayout * specific_metrics_layout = new QVBoxLayout(); + specific_metrics_widget->setLayout(specific_metrics_layout); + + // Add specific metric selector combobox + specific_metric_selector_ = new QComboBox(); + specific_metrics_layout->addWidget(specific_metric_selector_); + connect( + specific_metric_selector_, SIGNAL(currentIndexChanged(int)), this, + SLOT(onSpecificMetricChanged())); + + // Add clear button + QPushButton * clear_button = new QPushButton("Clear"); + specific_metrics_layout->addWidget(clear_button); + connect(clear_button, &QPushButton::clicked, this, &MetricsVisualizePanel::onClearButtonClicked); + + // Add chart view for specific metrics + specific_metric_chart_view_ = new QChartView(); + specific_metrics_layout->addWidget(specific_metric_chart_view_); + + tab_widget_->addTab( + specific_metrics_widget, "Specific Metrics"); // Add "Specific Metrics" tab to the tab widget + + // Set the main layout of the panel + QVBoxLayout * main_layout = new QVBoxLayout(); + main_layout->addWidget(tab_widget_); + setLayout(main_layout); } void MetricsVisualizePanel::onInitialize() @@ -38,14 +84,103 @@ void MetricsVisualizePanel::onInitialize() raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - sub_ = raw_node_->create_subscription( - "/diagnostic/planning_evaluator/metrics", rclcpp::QoS{1}, - std::bind(&MetricsVisualizePanel::onMetrics, this, _1)); + for (const auto & topic_name : topics_) { + const auto callback = [this, topic_name](const DiagnosticArray::ConstSharedPtr msg) { + this->onMetrics(msg, topic_name); + }; + const auto subscription = + raw_node_->create_subscription(topic_name, rclcpp::QoS{1}, callback); + subscriptions_[topic_name] = subscription; + } const auto period = std::chrono::milliseconds(static_cast(1e3 / 10)); timer_ = raw_node_->create_wall_timer(period, [&]() { onTimer(); }); } +void MetricsVisualizePanel::updateWidgetVisibility( + const std::string & target_topic, const bool show) +{ + for (const auto & [topic_name, metric_widgets_pair] : topic_widgets_map_) { + const bool is_target_topic = (topic_name == target_topic); + if ((!is_target_topic && show) || (is_target_topic && !show)) { + continue; + } + for (const auto & [metric, widgets] : metric_widgets_pair) { + widgets.first->setVisible(show); + widgets.second->setVisible(show); + } + } +} + +void MetricsVisualizePanel::showCurrentTopicWidgets() +{ + const std::string current_topic = topic_selector_->currentText().toStdString(); + updateWidgetVisibility(current_topic, true); +} + +void MetricsVisualizePanel::hideInactiveTopicWidgets() +{ + const std::string current_topic = topic_selector_->currentText().toStdString(); + updateWidgetVisibility(current_topic, false); +} + +void MetricsVisualizePanel::onTopicChanged() +{ + std::lock_guard message_lock(mutex_); + hideInactiveTopicWidgets(); + showCurrentTopicWidgets(); +} + +void MetricsVisualizePanel::updateSelectedMetric(const std::string & metric_name) +{ + std::lock_guard message_lock(mutex_); + + for (const auto & [topic, msg] : current_msg_map_) { + const auto time = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; + for (const auto & status : msg->status) { + if (metric_name == status.name) { + selected_metrics_ = {metric_name, Metric(status)}; + selected_metrics_->second.updateData(time, status); + return; + } + } + } +} + +void MetricsVisualizePanel::updateViews() +{ + if (!selected_metrics_) { + return; + } + + Metric & metric = selected_metrics_->second; + specific_metric_chart_view_->setChart(metric.getChartView()->chart()); + auto * specific_metrics_widget = dynamic_cast(tab_widget_->widget(1)); + auto * specific_metrics_layout = dynamic_cast(specific_metrics_widget->layout()); + specific_metrics_layout->removeWidget(specific_metric_table_); + specific_metric_table_ = metric.getTable(); + QSizePolicy sizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed); + sizePolicy.setHeightForWidth(specific_metric_table_->sizePolicy().hasHeightForWidth()); + specific_metric_table_->setSizePolicy(sizePolicy); + specific_metrics_layout->insertWidget(1, specific_metric_table_); +} + +void MetricsVisualizePanel::onSpecificMetricChanged() +{ + const auto selected_metrics_str = specific_metric_selector_->currentText().toStdString(); + updateSelectedMetric(selected_metrics_str); + updateViews(); +} + +void MetricsVisualizePanel::onClearButtonClicked() +{ + if (!selected_metrics_) { + return; + } + updateSelectedMetric(selected_metrics_->first); + updateViews(); +} + void MetricsVisualizePanel::onTimer() { std::lock_guard message_lock(mutex_); @@ -54,30 +189,71 @@ void MetricsVisualizePanel::onTimer() metric.updateGraph(); metric.updateTable(); } + + if (selected_metrics_) { + selected_metrics_->second.updateGraph(); + selected_metrics_->second.updateTable(); + } } -void MetricsVisualizePanel::onMetrics(const DiagnosticArray::ConstSharedPtr msg) +void MetricsVisualizePanel::onMetrics( + const DiagnosticArray::ConstSharedPtr & msg, const std::string & topic_name) { std::lock_guard message_lock(mutex_); const auto time = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; - constexpr size_t GRAPH_COL_SIZE = 5; - for (size_t i = 0; i < msg->status.size(); ++i) { - const auto & status = msg->status.at(i); + for (const auto & status : msg->status) { + const size_t num_current_metrics = topic_widgets_map_[topic_name].size(); if (metrics_.count(status.name) == 0) { - auto metric = Metric(status); + const auto metric = Metric(status); metrics_.emplace(status.name, metric); - grid_->addWidget(metric.getTable(), i / GRAPH_COL_SIZE * 2, i % GRAPH_COL_SIZE); - grid_->setRowStretch(i / GRAPH_COL_SIZE * 2, false); - grid_->addWidget(metric.getChartView(), i / GRAPH_COL_SIZE * 2 + 1, i % GRAPH_COL_SIZE); - grid_->setRowStretch(i / GRAPH_COL_SIZE * 2 + 1, true); - grid_->setColumnStretch(i % GRAPH_COL_SIZE, true); - } + // Calculate grid position + const size_t row = num_current_metrics / GRAPH_COL_SIZE * 2 + + 2; // start from 2 to leave space for the topic selector and tab widget + const size_t col = num_current_metrics % GRAPH_COL_SIZE; + + // Get the widgets from the metric + const auto tableWidget = metric.getTable(); + const auto chartViewWidget = metric.getChartView(); + + // Get the layout for the "All Metrics" tab + auto all_metrics_widget = dynamic_cast(tab_widget_->widget(0)); + QGridLayout * all_metrics_layout = dynamic_cast(all_metrics_widget->layout()); + + // Add the widgets to the "All Metrics" tab layout + all_metrics_layout->addWidget(tableWidget, row, col); + all_metrics_layout->setRowStretch(row, false); + all_metrics_layout->addWidget(chartViewWidget, row + 1, col); + all_metrics_layout->setRowStretch(row + 1, true); + all_metrics_layout->setColumnStretch(col, true); + + // Also add the widgets to the topic_widgets_map_ for easy management + topic_widgets_map_[topic_name][status.name] = std::make_pair(tableWidget, chartViewWidget); + } metrics_.at(status.name).updateData(time, status); + + // update selected metrics + const auto selected_metrics_str = specific_metric_selector_->currentText().toStdString(); + if (selected_metrics_str == status.name) { + if (selected_metrics_) { + selected_metrics_->second.updateData(time, status); + } + } } + + // Update the specific metric selector + QSignalBlocker blocker(specific_metric_selector_); + for (const auto & status : msg->status) { + if (specific_metric_selector_->findText(QString::fromStdString(status.name)) == -1) { + specific_metric_selector_->addItem(QString::fromStdString(status.name)); + } + } + + // save the message for metrics selector + current_msg_map_[topic_name] = msg; } } // namespace rviz_plugins diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index b49ff7bde2ed3..222ab33894afc 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -81,6 +81,7 @@ def launch_setup(context, *args, **kwargs): ("~/output/lateral_diagnostic", "lateral/diagnostic"), ("~/output/slope_angle", "longitudinal/slope_angle"), ("~/output/longitudinal_diagnostic", "longitudinal/diagnostic"), + ("~/output/stop_reason", "longitudinal/stop_reason"), ("~/output/control_cmd", "control_cmd"), ], parameters=[ diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index 09d61823235d8..f502aef017979 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -70,6 +70,18 @@ def get_vehicle_mirror_info(self): return p def create_additional_pipeline(self, lidar_name): + max_z = ( + self.vehicle_info["max_height_offset"] + + self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"][ + "margin_max_z" + ] + ) + min_z = ( + self.vehicle_info["min_height_offset"] + + self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"][ + "margin_min_z" + ] + ) components = [] components.append( ComposableNode( @@ -84,6 +96,8 @@ def create_additional_pipeline(self, lidar_name): { "input_frame": LaunchConfiguration("base_frame"), "output_frame": LaunchConfiguration("base_frame"), + "max_z": max_z, + "min_z": min_z, }, self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"], ], @@ -206,6 +220,14 @@ def create_ransac_pipeline(self): return components def create_common_pipeline(self, input_topic, output_topic): + max_z = ( + self.vehicle_info["max_height_offset"] + + self.ground_segmentation_param["common_crop_box_filter"]["parameters"]["margin_max_z"] + ) + min_z = ( + self.vehicle_info["min_height_offset"] + + self.ground_segmentation_param["common_crop_box_filter"]["parameters"]["margin_min_z"] + ) components = [] components.append( ComposableNode( @@ -220,6 +242,8 @@ def create_common_pipeline(self, input_topic, output_topic): { "input_frame": LaunchConfiguration("base_frame"), "output_frame": LaunchConfiguration("base_frame"), + "max_z": max_z, + "min_z": min_z, }, self.ground_segmentation_param["common_crop_box_filter"]["parameters"], ], diff --git a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml index fce3da96ce888..f99416cc5ac0a 100644 --- a/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml +++ b/launch/tier4_perception_launch/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml @@ -51,6 +51,7 @@ + diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 3801c448eaed4..3733cd09682fd 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -6,7 +6,7 @@ - + @@ -26,97 +26,100 @@ - - - - - + + + + + + - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + - + - - + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - + + + + - - - - - - - - - - - - - - + + + + + + + - - - - + + + + + + + + + + + + + + + + + + + + diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp index 5d05dd7e3755a..d62c2b38b6cd1 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp @@ -56,7 +56,9 @@ class TestArTagBasedLocalizer : public ::testing::Test TEST_F(TestArTagBasedLocalizer, test_setup) // NOLINT { // Check if the constructor finishes successfully - EXPECT_TRUE(true); + // For some unknown reason, the test sometimes fails to terminate normally and results in failure + // unless a 1-second wait is implemented. + std::this_thread::sleep_for(std::chrono::seconds(1)); } int main(int argc, char ** argv) diff --git a/localization/localization_util/src/smart_pose_buffer.cpp b/localization/localization_util/src/smart_pose_buffer.cpp index bc62bfa690946..201c743471efd 100644 --- a/localization/localization_util/src/smart_pose_buffer.cpp +++ b/localization/localization_util/src/smart_pose_buffer.cpp @@ -40,6 +40,7 @@ std::optional SmartPoseBuffer::interpolate( const rclcpp::Time time_last = pose_buffer_.back()->header.stamp; if (target_ros_time < time_first) { + RCLCPP_INFO(logger_, "Mismatch between pose timestamp and current timestamp"); return std::nullopt; } diff --git a/localization/localization_util/src/util_func.cpp b/localization/localization_util/src/util_func.cpp index bb32741067e65..ae6f0b61f063c 100644 --- a/localization/localization_util/src/util_func.cpp +++ b/localization/localization_util/src/util_func.cpp @@ -241,7 +241,7 @@ void output_pose_with_cov_to_log( rpy.y = rpy.y * 180.0 / M_PI; rpy.z = rpy.z * 180.0 / M_PI; - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( logger, std::fixed << prefix << "," << pose.position.x << "," << pose.position.y << "," << pose.position.z << "," << pose.orientation.x << "," << pose.orientation.y << "," << pose.orientation.z << "," << pose.orientation.w << "," << rpy.x diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 74d49e13a4c21..424a7051de883 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -60,6 +60,10 @@ One optional function is regularization. Please see the regularization chapter i {{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/frame.json") }} +#### Sensor Points + +{{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/sensor_points.json") }} + #### Ndt {{ json_to_markdown("localization/ndt_scan_matcher/schema/sub/ndt.json") }} diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index 144449ce75084..241892e67b66c 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -11,6 +11,12 @@ map_frame: "map" + sensor_points: + # Required distance of input sensor points. [m] + # If the max distance of input sensor points is lower than this value, the scan matching will not be performed. + required_distance: 10.0 + + ndt: # The maximum difference between two consecutive # transformations in order to consider convergence diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp index 2955e3cb9a5f7..ee1e2369c79bd 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp @@ -37,6 +37,11 @@ struct HyperParameters std::string map_frame; } frame; + struct SensorPoints + { + double required_distance; + } sensor_points; + pclomp::NdtParams ndt; bool ndt_regularization_enable; @@ -91,6 +96,9 @@ struct HyperParameters frame.ndt_base_frame = node->declare_parameter("frame.ndt_base_frame"); frame.map_frame = node->declare_parameter("frame.map_frame"); + sensor_points.required_distance = + node->declare_parameter("sensor_points.required_distance"); + ndt.trans_epsilon = node->declare_parameter("ndt.trans_epsilon"); ndt.step_size = node->declare_parameter("ndt.step_size"); ndt.resolution = node->declare_parameter("ndt.resolution"); diff --git a/localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json b/localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json index a42ee37858f46..74737a098e622 100644 --- a/localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json +++ b/localization/ndt_scan_matcher/schema/ndt_scan_matcher.schema.json @@ -21,6 +21,9 @@ "covariance": { "$ref": "sub/covariance.json#/definitions/covariance" }, "dynamic_map_loading": { "$ref": "sub/dynamic_map_loading.json#/definitions/dynamic_map_loading" + }, + "sensor_points": { + "$ref": "sub/sensor_points.json#/definitions/sensor_points" } }, "required": [ @@ -30,7 +33,8 @@ "validation", "score_estimation", "covariance", - "dynamic_map_loading" + "dynamic_map_loading", + "sensor_points" ], "additionalProperties": false } diff --git a/localization/ndt_scan_matcher/schema/sub/sensor_points.json b/localization/ndt_scan_matcher/schema/sub/sensor_points.json new file mode 100644 index 0000000000000..68a0b40f8f94e --- /dev/null +++ b/localization/ndt_scan_matcher/schema/sub/sensor_points.json @@ -0,0 +1,18 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ndt Scan Matcher Node", + "definitions": { + "sensor_points": { + "type": "object", + "properties": { + "required_distance": { + "type": "number", + "description": "Required distance of input sensor points [m]. If the max distance of input sensor points is lower than this value, the scan matching will not be performed.", + "default": "10.0" + } + }, + "required": ["required_distance"], + "additionalProperties": false + } + } +} diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 31d1c6588165f..d05292b2ad6c1 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -78,8 +78,19 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) ndt_ptr_->setParams(param); - update_ndt(position, *ndt_ptr_); - ndt_ptr_->setInputSource(input_source); + const bool updated = update_ndt(position, *ndt_ptr_); + if (!updated) { + RCLCPP_ERROR_STREAM_THROTTLE( + logger_, *clock_, 1000, + "update_ndt failed. If this happens with initial position estimation, make sure that" + "(1) the initial position matches the pcd map and (2) the map_loader is working properly."); + last_update_position_ = position; + ndt_ptr_mutex_->unlock(); + return; + } + if (input_source != nullptr) { + ndt_ptr_->setInputSource(input_source); + } ndt_ptr_mutex_->unlock(); need_rebuild_ = false; } else { @@ -98,7 +109,9 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) auto dummy_ptr = ndt_ptr_; auto input_source = ndt_ptr_->getInputSource(); ndt_ptr_ = secondary_ndt_ptr_; - ndt_ptr_->setInputSource(input_source); + if (input_source != nullptr) { + ndt_ptr_->setInputSource(input_source); + } ndt_ptr_mutex_->unlock(); dummy_ptr.reset(); @@ -173,7 +186,7 @@ bool MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt const auto duration_micro_sec = std::chrono::duration_cast(exe_end_time - exe_start_time).count(); const auto exe_time = static_cast(duration_micro_sec) / 1000.0; - RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time); + RCLCPP_DEBUG(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time); return true; // Updated } diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index cd637791f04b6..d8854757c2f8f 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -346,6 +346,21 @@ void NDTScanMatcher::callback_sensor_points( transform_sensor_measurement( sensor_frame, param_.frame.base_frame, sensor_points_in_sensor_frame, sensor_points_in_baselink_frame); + + // check max distance of sensor points + double max_distance = 0.0; + for (const auto & point : sensor_points_in_baselink_frame->points) { + const double distance = std::hypot(point.x, point.y, point.z); + max_distance = std::max(max_distance, distance); + } + if (max_distance < param_.sensor_points.required_distance) { + RCLCPP_WARN_STREAM( + this->get_logger(), "Max distance of sensor points = " + << std::fixed << std::setprecision(3) << max_distance << " [m] < " + << param_.sensor_points.required_distance << " [m]"); + return; + } + ndt_ptr_->setInputSource(sensor_points_in_baselink_frame); if (!is_activated_) return; @@ -992,7 +1007,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose; output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg); - RCLCPP_INFO_STREAM(get_logger(), "best_score," << best_particle_ptr->score); + RCLCPP_DEBUG_STREAM(get_logger(), "best_score," << best_particle_ptr->score); return result_pose_with_cov_msg; } diff --git a/localization/pose_initializer/src/pose_initializer/ndt_module.cpp b/localization/pose_initializer/src/pose_initializer/ndt_module.cpp index 801a08f83aab8..6e11f8fe74212 100644 --- a/localization/pose_initializer/src/pose_initializer/ndt_module.cpp +++ b/localization/pose_initializer/src/pose_initializer/ndt_module.cpp @@ -40,7 +40,6 @@ PoseWithCovarianceStamped NdtModule::align_pose(const PoseWithCovarianceStamped RCLCPP_INFO(logger_, "Call NDT align server."); const auto res = cli_align_->async_send_request(req).get(); if (!res->success) { - RCLCPP_INFO(logger_, "NDT align server failed."); throw ServiceException( Initialize::Service::Response::ERROR_ESTIMATION, "NDT align server failed."); } diff --git a/perception/bytetrack/package.xml b/perception/bytetrack/package.xml index da1768e1bf7ea..0d1ccc284f888 100644 --- a/perception/bytetrack/package.xml +++ b/perception/bytetrack/package.xml @@ -5,6 +5,7 @@ 0.0.1 ByteTrack implementation ported toward Autoware Manato HIRABAYASHI + Yoshi RI Apache License 2.0 ament_cmake_auto diff --git a/perception/bytetrack/src/bytetrack.cpp b/perception/bytetrack/src/bytetrack.cpp index 981bdb7f24afc..e02be6029ffaf 100644 --- a/perception/bytetrack/src/bytetrack.cpp +++ b/perception/bytetrack/src/bytetrack.cpp @@ -38,6 +38,7 @@ bool ByteTrack::do_inference(ObjectArray & objects) bytetrack_objects.emplace_back(bytetrack_obj); } + // cspell: ignore stracks tlwh // Update tracker std::vector output_stracks = tracker_->update(bytetrack_objects); diff --git a/perception/cluster_merger/package.xml b/perception/cluster_merger/package.xml index 14826ad07e098..220837bfd7e4e 100644 --- a/perception/cluster_merger/package.xml +++ b/perception/cluster_merger/package.xml @@ -6,8 +6,9 @@ The ROS 2 cluster merger package Yukihiro Saito Shunsuke Miura - autoware + Dai Nguyen Apache License 2.0 + Dai Nguyen ament_cmake_auto diff --git a/perception/compare_map_segmentation/package.xml b/perception/compare_map_segmentation/package.xml index 42fad723b5d57..ca427db9811ba 100644 --- a/perception/compare_map_segmentation/package.xml +++ b/perception/compare_map_segmentation/package.xml @@ -6,7 +6,7 @@ The ROS 2 compare_map_segmentation package amc-nu Yukihiro Saito - badai nguyen + Dai Nguyen Apache License 2.0 diff --git a/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp b/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp index 68a15113254d9..8476cd1262a58 100644 --- a/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp +++ b/perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp @@ -16,10 +16,13 @@ #define DETECTED_OBJECT_FEATURE_REMOVER__DETECTED_OBJECT_FEATURE_REMOVER_HPP_ #include +#include #include #include +#include + namespace detected_object_feature_remover { using autoware_auto_perception_msgs::msg::DetectedObjects; @@ -33,6 +36,7 @@ class DetectedObjectFeatureRemover : public rclcpp::Node private: rclcpp::Subscription::SharedPtr sub_; rclcpp::Publisher::SharedPtr pub_; + std::unique_ptr published_time_publisher_; void objectCallback(const DetectedObjectsWithFeature::ConstSharedPtr input); void convert(const DetectedObjectsWithFeature & objs_with_feature, DetectedObjects & objs); }; diff --git a/perception/detected_object_feature_remover/package.xml b/perception/detected_object_feature_remover/package.xml index 44571a3c18dd4..80d07291e17ce 100644 --- a/perception/detected_object_feature_remover/package.xml +++ b/perception/detected_object_feature_remover/package.xml @@ -13,6 +13,7 @@ autoware_auto_perception_msgs rclcpp rclcpp_components + tier4_autoware_utils tier4_perception_msgs ament_cmake_ros diff --git a/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp b/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp index d1e007d7ef65f..c5f977a4a3354 100644 --- a/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp +++ b/perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp @@ -23,6 +23,7 @@ DetectedObjectFeatureRemover::DetectedObjectFeatureRemover(const rclcpp::NodeOpt pub_ = this->create_publisher("~/output", rclcpp::QoS(1)); sub_ = this->create_subscription( "~/input", 1, std::bind(&DetectedObjectFeatureRemover::objectCallback, this, _1)); + published_time_publisher_ = std::make_unique(this); } void DetectedObjectFeatureRemover::objectCallback( @@ -31,6 +32,7 @@ void DetectedObjectFeatureRemover::objectCallback( DetectedObjects output; convert(*input, output); pub_->publish(output); + published_time_publisher_->publish_if_subscribed(pub_, output.header.stamp); } void DetectedObjectFeatureRemover::convert( diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp index e3c3263466033..f3871aaf98117 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -68,6 +69,8 @@ class ObjectLaneletFilterNode : public rclcpp::Node bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &); geometry_msgs::msg::Polygon setFootprint( const autoware_auto_perception_msgs::msg::DetectedObject &); + + std::unique_ptr published_time_publisher_; }; } // namespace object_lanelet_filter diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp index be25eb6a353e6..fda36f2c6b6d9 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -26,6 +27,7 @@ #include #include +#include #include namespace object_position_filter @@ -51,6 +53,8 @@ class ObjectPositionFilterNode : public rclcpp::Node float lower_bound_y_; utils::FilterTargetLabel filter_target_; bool isObjectInBounds(const autoware_auto_perception_msgs::msg::DetectedObject & object) const; + + std::unique_ptr published_time_publisher_; }; } // namespace object_position_filter diff --git a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index eb6da6bc45b24..742455c60a42c 100644 --- a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -148,6 +149,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node std::shared_ptr debugger_; bool using_2d_validator_; std::unique_ptr validator_; + std::unique_ptr published_time_publisher_; private: void onObjectsAndObstaclePointCloud( diff --git a/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp b/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp index 5fd866d09e725..9be990ebc1787 100644 --- a/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -42,6 +43,7 @@ class OccupancyGridBasedValidator : public rclcpp::Node message_filters::Subscriber occ_grid_sub_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; + std::unique_ptr published_time_publisher_; typedef message_filters::sync_policies::ApproximateTime< autoware_auto_perception_msgs::msg::DetectedObjects, nav_msgs::msg::OccupancyGrid> diff --git a/perception/detected_object_validation/package.xml b/perception/detected_object_validation/package.xml index 3e494a78cd5ad..f18f8ab922c49 100644 --- a/perception/detected_object_validation/package.xml +++ b/perception/detected_object_validation/package.xml @@ -6,8 +6,9 @@ The ROS 2 detected_object_validation package Yukihiro Saito Shunsuke Miura - badai nguyen + Dai Nguyen Shintaro Tomie + Yoshi RI Apache License 2.0 ament_cmake_auto diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index 74000a91e60fc..50a81e95d5a9b 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -55,6 +55,7 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod debug_publisher_ = std::make_unique(this, "object_lanelet_filter"); + published_time_publisher_ = std::make_unique(this); } void ObjectLaneletFilterNode::mapCallback( @@ -119,6 +120,7 @@ void ObjectLaneletFilterNode::objectCallback( ++index; } object_pub_->publish(output_object_msg); + published_time_publisher_->publish_if_subscribed(object_pub_, output_object_msg.header.stamp); // Publish debug info const double pipeline_latency = diff --git a/perception/detected_object_validation/src/object_position_filter.cpp b/perception/detected_object_validation/src/object_position_filter.cpp index a509fc7571dd5..eb3b42c83f125 100644 --- a/perception/detected_object_validation/src/object_position_filter.cpp +++ b/perception/detected_object_validation/src/object_position_filter.cpp @@ -42,6 +42,7 @@ ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & n "input/object", rclcpp::QoS{1}, std::bind(&ObjectPositionFilterNode::objectCallback, this, _1)); object_pub_ = this->create_publisher( "output/object", rclcpp::QoS{1}); + published_time_publisher_ = std::make_unique(this); } void ObjectPositionFilterNode::objectCallback( @@ -65,6 +66,7 @@ void ObjectPositionFilterNode::objectCallback( } object_pub_->publish(output_object_msg); + published_time_publisher_->publish_if_subscribed(object_pub_, output_object_msg.header.stamp); } bool ObjectPositionFilterNode::isObjectInBounds( diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index f3f56652792e9..e39de639a6121 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -309,6 +309,7 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( const bool enable_debugger = declare_parameter("enable_debugger", false); if (enable_debugger) debugger_ = std::make_shared(this); + published_time_publisher_ = std::make_unique(this); } void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, @@ -347,6 +348,7 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( } objects_pub_->publish(output); + published_time_publisher_->publish_if_subscribed(objects_pub_, output.header.stamp); if (debugger_) { debugger_->publishRemovedObjects(removed_objects); debugger_->publishNeighborPointcloud(input_obstacle_pointcloud->header); diff --git a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp index 80e4dc66d35c2..93bdb8eeee74c 100644 --- a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp +++ b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp @@ -52,6 +52,7 @@ OccupancyGridBasedValidator::OccupancyGridBasedValidator(const rclcpp::NodeOptio mean_threshold_ = declare_parameter("mean_threshold", 0.6); enable_debug_ = declare_parameter("enable_debug", false); + published_time_publisher_ = std::make_unique(this); } void OccupancyGridBasedValidator::onObjectsAndOccGrid( @@ -85,6 +86,7 @@ void OccupancyGridBasedValidator::onObjectsAndOccGrid( } objects_pub_->publish(output); + published_time_publisher_->publish_if_subscribed(objects_pub_, output.header.stamp); if (enable_debug_) showDebugImage(*input_occ_grid, transformed_objects, occ_grid); } diff --git a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp index fb7642356e8d9..0acba3d2dd58d 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp @@ -20,6 +20,8 @@ #include #include #include +#include +#include #include #include @@ -61,6 +63,11 @@ class Debugger divided_objects_pub_ = node->create_publisher( "debug/divided_objects", 1); + processing_time_publisher_ = + std::make_unique(node, "detection_by_tracker"); + stop_watch_ptr_ = + std::make_unique>(); + this->startStopWatch(); } ~Debugger() {} @@ -80,6 +87,19 @@ class Debugger { divided_objects_pub_->publish(removeFeature(input)); } + void startStopWatch() + { + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } + void startMeasureProcessingTime() { stop_watch_ptr_->tic("processing_time"); } + void publishProcessingTime() + { + processing_time_publisher_->publish( + "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); + processing_time_publisher_->publish( + "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); + } private: rclcpp::Publisher::SharedPtr @@ -90,6 +110,9 @@ class Debugger merged_objects_pub_; rclcpp::Publisher::SharedPtr divided_objects_pub_; + // debug publisher + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; autoware_auto_perception_msgs::msg::DetectedObjects removeFeature( const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input) diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp index 771747c80bb95..7747a37ee2f29 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -83,6 +84,8 @@ class DetectionByTracker : public rclcpp::Node detection_by_tracker::utils::TrackerIgnoreLabel tracker_ignore_; + std::unique_ptr published_time_publisher_; + void setMaxSearchRange(); void onObjects( diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp index 2595315e1f9b2..b1cc97ef3c391 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp @@ -177,6 +177,7 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) cluster_ = std::make_shared( false, 10, 10000, 0.7, 0.3, 0); debugger_ = std::make_shared(this); + published_time_publisher_ = std::make_unique(this); } void DetectionByTracker::setMaxSearchRange() @@ -206,6 +207,7 @@ void DetectionByTracker::setMaxSearchRange() void DetectionByTracker::onObjects( const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_msg) { + debugger_->startMeasureProcessingTime(); autoware_auto_perception_msgs::msg::DetectedObjects detected_objects; detected_objects.header = input_msg->header; @@ -220,6 +222,7 @@ void DetectionByTracker::onObjects( !object_recognition_utils::transformObjects( objects, input_msg->header.frame_id, tf_buffer_, transformed_objects)) { objects_pub_->publish(detected_objects); + published_time_publisher_->publish_if_subscribed(objects_pub_, detected_objects.header.stamp); return; } // to simplify post processes, convert tracked_objects to DetectedObjects message. @@ -250,6 +253,8 @@ void DetectionByTracker::onObjects( } objects_pub_->publish(detected_objects); + published_time_publisher_->publish_if_subscribed(objects_pub_, detected_objects.header.stamp); + debugger_->publishProcessingTime(); } void DetectionByTracker::divideUnderSegmentedObjects( diff --git a/perception/euclidean_cluster/package.xml b/perception/euclidean_cluster/package.xml index 56be3c3fc5bc0..58992d29d712d 100644 --- a/perception/euclidean_cluster/package.xml +++ b/perception/euclidean_cluster/package.xml @@ -5,6 +5,7 @@ 0.1.0 The euclidean_cluster package Yukihiro Saito + Dai Nguyen Apache License 2.0 ament_cmake_auto diff --git a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp index 980565fe2144b..d017d06929702 100644 --- a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp @@ -16,6 +16,7 @@ #define GROUND_SEGMENTATION__SCAN_GROUND_FILTER_NODELET_HPP_ #include "pointcloud_preprocessor/filter.hpp" +#include "pointcloud_preprocessor/transform_info.hpp" #include @@ -50,7 +51,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter // classified point label // (0: not classified, 1: ground, 2: not ground, 3: follow previous point, // 4: unkown(currently not used), 5: virtual ground) - enum class PointLabel { + enum class PointLabel : uint16_t { INIT = 0, GROUND, NON_GROUND, @@ -59,19 +60,15 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter VIRTUAL_GROUND, OUT_OF_RANGE }; - struct PointRef + + struct PointData { - float grid_size; // radius of grid - uint16_t grid_id; // id of grid in vertical - float radius; // cylindrical coords on XY Plane - float theta; // angle deg on XY plane - size_t radial_div; // index of the radial division to which this point belongs to + float radius; // cylindrical coords on XY Plane PointLabel point_state{PointLabel::INIT}; - + uint16_t grid_id; // id of grid in vertical size_t orig_index; // index of this point in the source pointcloud - pcl::PointXYZ * orig_point; }; - using PointCloudRefVector = std::vector; + using PointCloudVector = std::vector; struct GridCenter { @@ -144,18 +141,39 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter pcl::PointIndices getIndices() { return pcl_indices; } std::vector getHeightList() { return height_list; } + + pcl::PointIndices & getIndicesRef() { return pcl_indices; } + std::vector & getHeightListRef() { return height_list; } }; void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; + // TODO(taisa1): Temporary Implementation: Remove this interface when all the filter nodes + // conform to new API + virtual void faster_filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, + const pointcloud_preprocessor::TransformInfo & transform_info); + tf2_ros::Buffer tf_buffer_{get_clock()}; tf2_ros::TransformListener tf_listener_{tf_buffer_}; + int x_offset_; + int y_offset_; + int z_offset_; + int intensity_offset_; + bool offset_initialized_; + + void set_field_offsets(const PointCloud2ConstPtr & input); + + void get_point_from_global_offset( + const PointCloud2ConstPtr & input, pcl::PointXYZ & point, size_t global_offset); + const uint16_t gnd_grid_continual_thresh_ = 3; bool elevation_grid_mode_; float non_ground_height_threshold_; float grid_size_rad_; + float tan_grid_size_rad_; float grid_size_m_; float low_priority_region_x_; uint16_t gnd_grid_buffer_size_; @@ -163,14 +181,17 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter float grid_mode_switch_angle_rad_; float virtual_lidar_z_; float detection_range_z_max_; - float center_pcl_shift_; // virtual center of pcl to center mass - float grid_mode_switch_radius_; // non linear grid size switching distance - double global_slope_max_angle_rad_; // radians - double local_slope_max_angle_rad_; // radians + float center_pcl_shift_; // virtual center of pcl to center mass + float grid_mode_switch_radius_; // non linear grid size switching distance + double global_slope_max_angle_rad_; // radians + double local_slope_max_angle_rad_; // radians + double global_slope_max_ratio_; + double local_slope_max_ratio_; double radial_divider_angle_rad_; // distance in rads between dividers double split_points_distance_tolerance_; // distance in meters between concentric divisions - double // minimum height threshold regardless the slope, - split_height_distance_; // useful for close points + double split_points_distance_tolerance_square_; + double // minimum height threshold regardless the slope, + split_height_distance_; // useful for close points bool use_virtual_ground_point_; bool use_recheck_ground_cluster_; // to enable recheck ground cluster size_t radial_dividers_num_; @@ -186,23 +207,25 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter */ /*! - * Convert pcl::PointCloud to sorted PointCloudRefVector + * Convert sensor_msgs::msg::PointCloud2 to sorted PointCloudVector * @param[in] in_cloud Input Point Cloud to be organized in radial segments * @param[out] out_radial_ordered_points_manager Vector of Points Clouds, * each element will contain the points ordered */ void convertPointcloud( - const pcl::PointCloud::Ptr in_cloud, - std::vector & out_radial_ordered_points_manager); + const PointCloud2ConstPtr & in_cloud, + std::vector & out_radial_ordered_points_manager); void convertPointcloudGridScan( - const pcl::PointCloud::Ptr in_cloud, - std::vector & out_radial_ordered_points_manager); + const PointCloud2ConstPtr & in_cloud, + std::vector & out_radial_ordered_points_manager); /*! * Output ground center of front wheels as the virtual ground point * @param[out] point Virtual ground origin point */ void calcVirtualGroundOrigin(pcl::PointXYZ & point); + float calcGridSize(const PointData & p); + /*! * Classifies Points in the PointCloud as Ground and Not Ground * @param in_radial_ordered_clouds Vector of an Ordered PointsCloud @@ -214,14 +237,19 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter void initializeFirstGndGrids( const float h, const float r, const uint16_t id, std::vector & gnd_grids); - void checkContinuousGndGrid(PointRef & p, const std::vector & gnd_grids_list); - void checkDiscontinuousGndGrid(PointRef & p, const std::vector & gnd_grids_list); - void checkBreakGndGrid(PointRef & p, const std::vector & gnd_grids_list); + void checkContinuousGndGrid( + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list); + void checkDiscontinuousGndGrid( + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list); + void checkBreakGndGrid( + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list); void classifyPointCloud( - std::vector & in_radial_ordered_clouds, + const PointCloud2ConstPtr & in_cloud_ptr, + std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices); void classifyPointCloudGridScan( - std::vector & in_radial_ordered_clouds, + const PointCloud2ConstPtr & in_cloud_ptr, + std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices); /*! * Re-classifies point of ground cluster based on their height @@ -237,11 +265,11 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter * and the other removed as indicated in the indices * @param in_cloud_ptr Input PointCloud to which the extraction will be performed * @param in_indices Indices of the points to be both removed and kept - * @param out_object_cloud_ptr Resulting PointCloud with the indices kept + * @param out_object_cloud Resulting PointCloud with the indices kept */ void extractObjectPoints( - const pcl::PointCloud::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices, - pcl::PointCloud::Ptr out_object_cloud_ptr); + const PointCloud2ConstPtr & in_cloud_ptr, const pcl::PointIndices & in_indices, + PointCloud2 & out_object_cloud); /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; diff --git a/perception/ground_segmentation/package.xml b/perception/ground_segmentation/package.xml index 84a053d732cc9..1734487dcc6b3 100644 --- a/perception/ground_segmentation/package.xml +++ b/perception/ground_segmentation/package.xml @@ -7,7 +7,7 @@ amc-nu Yukihiro Saito Shunsuke Miura - badai nguyen + Dai Nguyen Apache License 2.0 diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index f164e0102e0e9..7a0333d95075b 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -48,8 +48,12 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & elevation_grid_mode_ = declare_parameter("elevation_grid_mode"); global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg")); local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg")); + global_slope_max_ratio_ = std::tan(global_slope_max_angle_rad_); + local_slope_max_ratio_ = std::tan(local_slope_max_angle_rad_); radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg")); split_points_distance_tolerance_ = declare_parameter("split_points_distance_tolerance"); + split_points_distance_tolerance_square_ = + split_points_distance_tolerance_ * split_points_distance_tolerance_; split_height_distance_ = declare_parameter("split_height_distance"); use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point"); use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster"); @@ -60,6 +64,12 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & grid_mode_switch_radius_ / grid_size_m_; // changing the mode of grid division virtual_lidar_z_ = vehicle_info_.vehicle_height_m; grid_mode_switch_angle_rad_ = std::atan2(grid_mode_switch_radius_, virtual_lidar_z_); + + grid_size_rad_ = + normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - + normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); + tan_grid_size_rad_ = std::tan(grid_size_rad_); + offset_initialized_ = false; } using std::placeholders::_1; @@ -77,92 +87,117 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & } } +inline void ScanGroundFilterComponent::set_field_offsets(const PointCloud2ConstPtr & input) +{ + x_offset_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; + y_offset_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; + z_offset_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; + int intensity_index = pcl::getFieldIndex(*input, "intensity"); + if (intensity_index != -1) { + intensity_offset_ = input->fields[intensity_index].offset; + } else { + intensity_offset_ = z_offset_ + sizeof(float); + } + offset_initialized_ = true; +} + +inline void ScanGroundFilterComponent::get_point_from_global_offset( + const PointCloud2ConstPtr & input, pcl::PointXYZ & point, size_t global_offset) +{ + point.x = *reinterpret_cast(&input->data[global_offset + x_offset_]); + point.y = *reinterpret_cast(&input->data[global_offset + y_offset_]); + point.z = *reinterpret_cast(&input->data[global_offset + z_offset_]); +} + void ScanGroundFilterComponent::convertPointcloudGridScan( - const pcl::PointCloud::Ptr in_cloud, - std::vector & out_radial_ordered_points) + const PointCloud2ConstPtr & in_cloud, std::vector & out_radial_ordered_points) { out_radial_ordered_points.resize(radial_dividers_num_); - PointRef current_point; - uint16_t back_steps_num = 1; + PointData current_point; + + const auto inv_radial_divider_angle_rad = 1.0f / radial_divider_angle_rad_; + const auto inv_grid_size_rad = 1.0f / grid_size_rad_; + const auto inv_grid_size_m = 1.0f / grid_size_m_; + + const auto grid_id_offset = + grid_mode_switch_grid_id_ - grid_mode_switch_angle_rad_ * inv_grid_size_rad; + const auto x_shift = vehicle_info_.wheel_base_m / 2.0f + center_pcl_shift_; - grid_size_rad_ = - normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - - normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); - for (size_t i = 0; i < in_cloud->points.size(); ++i) { - auto x{ - in_cloud->points[i].x - vehicle_info_.wheel_base_m / 2.0f - - center_pcl_shift_}; // base on front wheel center - // auto y{in_cloud->points[i].y}; - auto radius{static_cast(std::hypot(x, in_cloud->points[i].y))}; - auto theta{normalizeRadian(std::atan2(x, in_cloud->points[i].y), 0.0)}; + const size_t in_cloud_data_size = in_cloud->data.size(); + const size_t in_cloud_point_step = in_cloud->point_step; + + size_t point_index = 0; + pcl::PointXYZ input_point; + for (size_t global_offset = 0; global_offset + in_cloud_point_step <= in_cloud_data_size; + global_offset += in_cloud_point_step) { + get_point_from_global_offset(in_cloud, input_point, global_offset); + + auto x{input_point.x - x_shift}; // base on front wheel center + auto radius{static_cast(std::hypot(x, input_point.y))}; + auto theta{normalizeRadian(std::atan2(x, input_point.y), 0.0)}; // divide by vertical angle - auto gamma{normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f)}; - auto radial_div{ - static_cast(std::floor(normalizeDegree(theta / radial_divider_angle_rad_, 0.0)))}; + auto radial_div{static_cast(std::floor(theta * inv_radial_divider_angle_rad))}; uint16_t grid_id = 0; - float curr_grid_size = 0.0f; if (radius <= grid_mode_switch_radius_) { - grid_id = static_cast(radius / grid_size_m_); - curr_grid_size = grid_size_m_; + grid_id = static_cast(radius * inv_grid_size_m); } else { - grid_id = grid_mode_switch_grid_id_ + (gamma - grid_mode_switch_angle_rad_) / grid_size_rad_; - if (grid_id <= grid_mode_switch_grid_id_ + back_steps_num) { - curr_grid_size = grid_size_m_; - } else { - curr_grid_size = std::tan(gamma) - std::tan(gamma - grid_size_rad_); - curr_grid_size *= virtual_lidar_z_; - } + auto gamma{normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f)}; + grid_id = grid_id_offset + gamma * inv_grid_size_rad; } current_point.grid_id = grid_id; - current_point.grid_size = curr_grid_size; current_point.radius = radius; - current_point.theta = theta; - current_point.radial_div = radial_div; current_point.point_state = PointLabel::INIT; - current_point.orig_index = i; - current_point.orig_point = &in_cloud->points[i]; + current_point.orig_index = point_index; // radial divisions out_radial_ordered_points[radial_div].emplace_back(current_point); + ++point_index; } // sort by distance for (size_t i = 0; i < radial_dividers_num_; ++i) { std::sort( out_radial_ordered_points[i].begin(), out_radial_ordered_points[i].end(), - [](const PointRef & a, const PointRef & b) { return a.radius < b.radius; }); + [](const PointData & a, const PointData & b) { return a.radius < b.radius; }); } } + void ScanGroundFilterComponent::convertPointcloud( - const pcl::PointCloud::Ptr in_cloud, - std::vector & out_radial_ordered_points) + const PointCloud2ConstPtr & in_cloud, std::vector & out_radial_ordered_points) { out_radial_ordered_points.resize(radial_dividers_num_); - PointRef current_point; + PointData current_point; + + const auto inv_radial_divider_angle_rad = 1.0f / radial_divider_angle_rad_; + + const size_t in_cloud_data_size = in_cloud->data.size(); + const size_t in_cloud_point_step = in_cloud->point_step; - for (size_t i = 0; i < in_cloud->points.size(); ++i) { - auto radius{static_cast(std::hypot(in_cloud->points[i].x, in_cloud->points[i].y))}; - auto theta{normalizeRadian(std::atan2(in_cloud->points[i].x, in_cloud->points[i].y), 0.0)}; - auto radial_div{ - static_cast(std::floor(normalizeDegree(theta / radial_divider_angle_rad_, 0.0)))}; + size_t point_index = 0; + pcl::PointXYZ input_point; + for (size_t global_offset = 0; global_offset + in_cloud_point_step <= in_cloud_data_size; + global_offset += in_cloud_point_step) { + // Point + get_point_from_global_offset(in_cloud, input_point, global_offset); + + auto radius{static_cast(std::hypot(input_point.x, input_point.y))}; + auto theta{normalizeRadian(std::atan2(input_point.x, input_point.y), 0.0)}; + auto radial_div{static_cast(std::floor(theta * inv_radial_divider_angle_rad))}; current_point.radius = radius; - current_point.theta = theta; - current_point.radial_div = radial_div; current_point.point_state = PointLabel::INIT; - current_point.orig_index = i; - current_point.orig_point = &in_cloud->points[i]; + current_point.orig_index = point_index; // radial divisions out_radial_ordered_points[radial_div].emplace_back(current_point); + ++point_index; } - // sort by distance for (size_t i = 0; i < radial_dividers_num_; ++i) { std::sort( out_radial_ordered_points[i].begin(), out_radial_ordered_points[i].end(), - [](const PointRef & a, const PointRef & b) { return a.radius < b.radius; }); + [](const PointData & a, const PointData & b) { return a.radius < b.radius; }); } } @@ -173,6 +208,21 @@ void ScanGroundFilterComponent::calcVirtualGroundOrigin(pcl::PointXYZ & point) point.z = 0; } +inline float ScanGroundFilterComponent::calcGridSize(const PointData & p) +{ + float grid_size = grid_size_m_; + uint16_t back_steps_num = 1; + + if ( + p.radius > grid_mode_switch_radius_ && p.grid_id > grid_mode_switch_grid_id_ + back_steps_num) { + // equivalent to grid_size = (std::tan(gamma) - std::tan(gamma - grid_size_rad_)) * + // virtual_lidar_z_ when gamma = normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f) + grid_size = p.radius - (p.radius - tan_grid_size_rad_ * virtual_lidar_z_) / + (1 + p.radius * tan_grid_size_rad_ / virtual_lidar_z_); + } + return grid_size; +} + void ScanGroundFilterComponent::initializeFirstGndGrids( const float h, const float r, const uint16_t id, std::vector & gnd_grids) { @@ -193,96 +243,94 @@ void ScanGroundFilterComponent::initializeFirstGndGrids( } void ScanGroundFilterComponent::checkContinuousGndGrid( - PointRef & p, const std::vector & gnd_grids_list) + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) { float next_gnd_z = 0.0f; - float curr_gnd_slope_rad = 0.0f; + float curr_gnd_slope_ratio = 0.0f; float gnd_buff_z_mean = 0.0f; - float gnd_buff_z_max = 0.0f; float gnd_buff_radius = 0.0f; for (auto it = gnd_grids_list.end() - gnd_grid_buffer_size_ - 1; it < gnd_grids_list.end() - 1; ++it) { gnd_buff_radius += it->radius; gnd_buff_z_mean += it->avg_height; - gnd_buff_z_max += it->max_height; } gnd_buff_radius /= static_cast(gnd_grid_buffer_size_ - 1); - gnd_buff_z_max /= static_cast(gnd_grid_buffer_size_ - 1); gnd_buff_z_mean /= static_cast(gnd_grid_buffer_size_ - 1); float tmp_delta_mean_z = gnd_grids_list.back().avg_height - gnd_buff_z_mean; float tmp_delta_radius = gnd_grids_list.back().radius - gnd_buff_radius; - curr_gnd_slope_rad = std::atan(tmp_delta_mean_z / tmp_delta_radius); - curr_gnd_slope_rad = curr_gnd_slope_rad < -global_slope_max_angle_rad_ - ? -global_slope_max_angle_rad_ - : curr_gnd_slope_rad; - curr_gnd_slope_rad = curr_gnd_slope_rad > global_slope_max_angle_rad_ - ? global_slope_max_angle_rad_ - : curr_gnd_slope_rad; + curr_gnd_slope_ratio = tmp_delta_mean_z / tmp_delta_radius; + curr_gnd_slope_ratio = curr_gnd_slope_ratio < -global_slope_max_ratio_ ? -global_slope_max_ratio_ + : curr_gnd_slope_ratio; + curr_gnd_slope_ratio = + curr_gnd_slope_ratio > global_slope_max_ratio_ ? global_slope_max_ratio_ : curr_gnd_slope_ratio; - next_gnd_z = std::tan(curr_gnd_slope_rad) * (p.radius - gnd_buff_radius) + gnd_buff_z_mean; + next_gnd_z = curr_gnd_slope_ratio * (p.radius - gnd_buff_radius) + gnd_buff_z_mean; float gnd_z_local_thresh = std::tan(DEG2RAD(5.0)) * (p.radius - gnd_grids_list.back().radius); - tmp_delta_mean_z = p.orig_point->z - (gnd_grids_list.end() - 2)->avg_height; + tmp_delta_mean_z = p_orig_point.z - (gnd_grids_list.end() - 2)->avg_height; tmp_delta_radius = p.radius - (gnd_grids_list.end() - 2)->radius; - float local_slope = std::atan(tmp_delta_mean_z / tmp_delta_radius); + float local_slope_ratio = tmp_delta_mean_z / tmp_delta_radius; if ( - abs(p.orig_point->z - next_gnd_z) <= non_ground_height_threshold_ + gnd_z_local_thresh || - abs(local_slope) <= local_slope_max_angle_rad_) { + abs(p_orig_point.z - next_gnd_z) <= non_ground_height_threshold_ + gnd_z_local_thresh || + abs(local_slope_ratio) <= local_slope_max_ratio_) { p.point_state = PointLabel::GROUND; - } else if (p.orig_point->z - next_gnd_z > non_ground_height_threshold_ + gnd_z_local_thresh) { + } else if (p_orig_point.z - next_gnd_z > non_ground_height_threshold_ + gnd_z_local_thresh) { p.point_state = PointLabel::NON_GROUND; } } + void ScanGroundFilterComponent::checkDiscontinuousGndGrid( - PointRef & p, const std::vector & gnd_grids_list) + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) { - float tmp_delta_max_z = p.orig_point->z - gnd_grids_list.back().max_height; - float tmp_delta_avg_z = p.orig_point->z - gnd_grids_list.back().avg_height; + float tmp_delta_max_z = p_orig_point.z - gnd_grids_list.back().max_height; + float tmp_delta_avg_z = p_orig_point.z - gnd_grids_list.back().avg_height; float tmp_delta_radius = p.radius - gnd_grids_list.back().radius; - float local_slope = std::atan(tmp_delta_avg_z / tmp_delta_radius); + float local_slope_ratio = tmp_delta_avg_z / tmp_delta_radius; if ( - abs(local_slope) < local_slope_max_angle_rad_ || + abs(local_slope_ratio) < local_slope_max_ratio_ || abs(tmp_delta_avg_z) < non_ground_height_threshold_ || abs(tmp_delta_max_z) < non_ground_height_threshold_) { p.point_state = PointLabel::GROUND; - } else if (local_slope > global_slope_max_angle_rad_) { + } else if (local_slope_ratio > global_slope_max_ratio_) { p.point_state = PointLabel::NON_GROUND; } } void ScanGroundFilterComponent::checkBreakGndGrid( - PointRef & p, const std::vector & gnd_grids_list) + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) { - float tmp_delta_avg_z = p.orig_point->z - gnd_grids_list.back().avg_height; + float tmp_delta_avg_z = p_orig_point.z - gnd_grids_list.back().avg_height; float tmp_delta_radius = p.radius - gnd_grids_list.back().radius; - float local_slope = std::atan(tmp_delta_avg_z / tmp_delta_radius); - if (abs(local_slope) < global_slope_max_angle_rad_) { + float local_slope_ratio = tmp_delta_avg_z / tmp_delta_radius; + if (abs(local_slope_ratio) < global_slope_max_ratio_) { p.point_state = PointLabel::GROUND; - } else if (local_slope > global_slope_max_angle_rad_) { + } else if (local_slope_ratio > global_slope_max_ratio_) { p.point_state = PointLabel::NON_GROUND; } } + void ScanGroundFilterComponent::recheckGroundCluster( PointsCentroid & gnd_cluster, const float non_ground_threshold, pcl::PointIndices & non_ground_indices) { const float min_gnd_height = gnd_cluster.getMinHeight(); - pcl::PointIndices gnd_indices = gnd_cluster.getIndices(); - std::vector height_list = gnd_cluster.getHeightList(); + const pcl::PointIndices & gnd_indices = gnd_cluster.getIndicesRef(); + const std::vector & height_list = gnd_cluster.getHeightListRef(); for (size_t i = 0; i < height_list.size(); ++i) { if (height_list.at(i) >= min_gnd_height + non_ground_threshold) { non_ground_indices.indices.push_back(gnd_indices.indices.at(i)); } } } + void ScanGroundFilterComponent::classifyPointCloudGridScan( - std::vector & in_radial_ordered_clouds, + const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices) { out_no_ground_indices.indices.clear(); @@ -299,42 +347,41 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( // check the first point in ray auto * p = &in_radial_ordered_clouds[i][0]; - PointRef * prev_p; - prev_p = &in_radial_ordered_clouds[i][0]; // for checking the distance to prev point + auto * prev_p = &in_radial_ordered_clouds[i][0]; // for checking the distance to prev point bool initialized_first_gnd_grid = false; bool prev_list_init = false; - - for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); ++j) { - p = &in_radial_ordered_clouds[i][j]; - float global_slope_p = std::atan(p->orig_point->z / p->radius); + pcl::PointXYZ p_orig_point, prev_p_orig_point; + for (auto & point : in_radial_ordered_clouds[i]) { + prev_p = p; + prev_p_orig_point = p_orig_point; + p = &point; + get_point_from_global_offset(in_cloud, p_orig_point, in_cloud->point_step * p->orig_index); + float global_slope_ratio_p = p_orig_point.z / p->radius; float non_ground_height_threshold_local = non_ground_height_threshold_; - if (p->orig_point->x < low_priority_region_x_) { + if (p_orig_point.x < low_priority_region_x_) { non_ground_height_threshold_local = - non_ground_height_threshold_ * abs(p->orig_point->x / low_priority_region_x_); + non_ground_height_threshold_ * abs(p_orig_point.x / low_priority_region_x_); } // classify first grid's point cloud if ( - !initialized_first_gnd_grid && global_slope_p >= global_slope_max_angle_rad_ && - p->orig_point->z > non_ground_height_threshold_local) { + !initialized_first_gnd_grid && global_slope_ratio_p >= global_slope_max_ratio_ && + p_orig_point.z > non_ground_height_threshold_local) { out_no_ground_indices.indices.push_back(p->orig_index); p->point_state = PointLabel::NON_GROUND; - prev_p = p; continue; } if ( - !initialized_first_gnd_grid && abs(global_slope_p) < global_slope_max_angle_rad_ && - abs(p->orig_point->z) < non_ground_height_threshold_local) { - ground_cluster.addPoint(p->radius, p->orig_point->z, p->orig_index); + !initialized_first_gnd_grid && abs(global_slope_ratio_p) < global_slope_max_ratio_ && + abs(p_orig_point.z) < non_ground_height_threshold_local) { + ground_cluster.addPoint(p->radius, p_orig_point.z, p->orig_index); p->point_state = PointLabel::GROUND; initialized_first_gnd_grid = static_cast(p->grid_id - prev_p->grid_id); - prev_p = p; continue; } if (!initialized_first_gnd_grid) { - prev_p = p; continue; } @@ -366,53 +413,50 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( ground_cluster.initialize(); } // classify - if (p->orig_point->z - gnd_grids.back().avg_height > detection_range_z_max_) { + if (p_orig_point.z - gnd_grids.back().avg_height > detection_range_z_max_) { p->point_state = PointLabel::OUT_OF_RANGE; - prev_p = p; continue; } - float points_xy_distance = std::hypot( - p->orig_point->x - prev_p->orig_point->x, p->orig_point->y - prev_p->orig_point->y); + float points_xy_distance_square = + (p_orig_point.x - prev_p_orig_point.x) * (p_orig_point.x - prev_p_orig_point.x) + + (p_orig_point.y - prev_p_orig_point.y) * (p_orig_point.y - prev_p_orig_point.y); if ( prev_p->point_state == PointLabel::NON_GROUND && - points_xy_distance < split_points_distance_tolerance_ && - p->orig_point->z > prev_p->orig_point->z) { + points_xy_distance_square < split_points_distance_tolerance_square_ && + p_orig_point.z > prev_p_orig_point.z) { p->point_state = PointLabel::NON_GROUND; out_no_ground_indices.indices.push_back(p->orig_index); - prev_p = p; continue; } - - if (global_slope_p > global_slope_max_angle_rad_) { + if (global_slope_ratio_p > global_slope_max_ratio_) { out_no_ground_indices.indices.push_back(p->orig_index); - prev_p = p; continue; } // gnd grid is continuous, the last gnd grid is close uint16_t next_gnd_grid_id_thresh = (gnd_grids.end() - gnd_grid_buffer_size_)->grid_id + gnd_grid_buffer_size_ + gnd_grid_continual_thresh_; + float curr_grid_size = calcGridSize(*p); if ( p->grid_id < next_gnd_grid_id_thresh && - p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * p->grid_size) { - checkContinuousGndGrid(*p, gnd_grids); - - } else if (p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * p->grid_size) { - checkDiscontinuousGndGrid(*p, gnd_grids); + p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * curr_grid_size) { + checkContinuousGndGrid(*p, p_orig_point, gnd_grids); + } else if ( + p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * curr_grid_size) { + checkDiscontinuousGndGrid(*p, p_orig_point, gnd_grids); } else { - checkBreakGndGrid(*p, gnd_grids); + checkBreakGndGrid(*p, p_orig_point, gnd_grids); } if (p->point_state == PointLabel::NON_GROUND) { out_no_ground_indices.indices.push_back(p->orig_index); } else if (p->point_state == PointLabel::GROUND) { - ground_cluster.addPoint(p->radius, p->orig_point->z, p->orig_index); + ground_cluster.addPoint(p->radius, p_orig_point.z, p->orig_index); } - prev_p = p; } } } void ScanGroundFilterComponent::classifyPointCloud( - std::vector & in_radial_ordered_clouds, + const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices) { out_no_ground_indices.indices.clear(); @@ -430,16 +474,15 @@ void ScanGroundFilterComponent::classifyPointCloud( PointsCentroid ground_cluster, non_ground_cluster; float local_slope = 0.0f; PointLabel prev_point_label = PointLabel::INIT; - pcl::PointXYZ prev_gnd_point(0, 0, 0); + pcl::PointXYZ prev_gnd_point(0, 0, 0), p_orig_point, prev_p_orig_point; // loop through each point in the radial div for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); ++j) { - const float global_slope_max_angle = global_slope_max_angle_rad_; const float local_slope_max_angle = local_slope_max_angle_rad_; + prev_p_orig_point = p_orig_point; auto * p = &in_radial_ordered_clouds[i][j]; - auto * p_prev = &in_radial_ordered_clouds[i][j - 1]; - + get_point_from_global_offset(in_cloud, p_orig_point, in_cloud->point_step * p->orig_index); if (j == 0) { - bool is_front_side = (p->orig_point->x > virtual_ground_point.x); + bool is_front_side = (p_orig_point.x > virtual_ground_point.x); if (use_virtual_ground_point_ && is_front_side) { prev_gnd_point = virtual_ground_point; } else { @@ -449,22 +492,22 @@ void ScanGroundFilterComponent::classifyPointCloud( prev_gnd_slope = 0.0f; ground_cluster.initialize(); non_ground_cluster.initialize(); - points_distance = calcDistance3d(*p->orig_point, prev_gnd_point); + points_distance = calcDistance3d(p_orig_point, prev_gnd_point); } else { - points_distance = calcDistance3d(*p->orig_point, *p_prev->orig_point); + points_distance = calcDistance3d(p_orig_point, prev_p_orig_point); } float radius_distance_from_gnd = p->radius - prev_gnd_radius; - float height_from_gnd = p->orig_point->z - prev_gnd_point.z; - float height_from_obj = p->orig_point->z - non_ground_cluster.getAverageHeight(); + float height_from_gnd = p_orig_point.z - prev_gnd_point.z; + float height_from_obj = p_orig_point.z - non_ground_cluster.getAverageHeight(); bool calculate_slope = false; bool is_point_close_to_prev = (points_distance < (p->radius * radial_divider_angle_rad_ + split_points_distance_tolerance_)); - float global_slope = std::atan2(p->orig_point->z, p->radius); + float global_slope_ratio = p_orig_point.z / p->radius; // check points which is far enough from previous point - if (global_slope > global_slope_max_angle) { + if (global_slope_ratio > global_slope_max_ratio_) { p->point_state = PointLabel::NON_GROUND; calculate_slope = false; } else if ( @@ -479,7 +522,7 @@ void ScanGroundFilterComponent::classifyPointCloud( calculate_slope = true; } if (is_point_close_to_prev) { - height_from_gnd = p->orig_point->z - ground_cluster.getAverageHeight(); + height_from_gnd = p_orig_point.z - ground_cluster.getAverageHeight(); radius_distance_from_gnd = p->radius - ground_cluster.getAverageRadius(); } if (calculate_slope) { @@ -514,58 +557,66 @@ void ScanGroundFilterComponent::classifyPointCloud( prev_point_label = p->point_state; if (p->point_state == PointLabel::GROUND) { prev_gnd_radius = p->radius; - prev_gnd_point = pcl::PointXYZ(p->orig_point->x, p->orig_point->y, p->orig_point->z); - ground_cluster.addPoint(p->radius, p->orig_point->z); + prev_gnd_point = pcl::PointXYZ(p_orig_point.x, p_orig_point.y, p_orig_point.z); + ground_cluster.addPoint(p->radius, p_orig_point.z); prev_gnd_slope = ground_cluster.getAverageSlope(); } // update the non ground state if (p->point_state == PointLabel::NON_GROUND) { - non_ground_cluster.addPoint(p->radius, p->orig_point->z); + non_ground_cluster.addPoint(p->radius, p_orig_point.z); } } } } void ScanGroundFilterComponent::extractObjectPoints( - const pcl::PointCloud::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices, - pcl::PointCloud::Ptr out_object_cloud_ptr) + const PointCloud2ConstPtr & in_cloud_ptr, const pcl::PointIndices & in_indices, + PointCloud2 & out_object_cloud) { + size_t output_data_size = 0; for (const auto & i : in_indices.indices) { - out_object_cloud_ptr->points.emplace_back(in_cloud_ptr->points[i]); + std::memcpy( + &out_object_cloud.data[output_data_size], &in_cloud_ptr->data[i * in_cloud_ptr->point_step], + in_cloud_ptr->point_step * sizeof(uint8_t)); + *reinterpret_cast(&out_object_cloud.data[output_data_size + intensity_offset_]) = + 1; // set intensity to 1 + output_data_size += in_cloud_ptr->point_step; } } -void ScanGroundFilterComponent::filter( +void ScanGroundFilterComponent::faster_filter( const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output) + PointCloud2 & output, + [[maybe_unused]] const pointcloud_preprocessor::TransformInfo & transform_info) { std::scoped_lock lock(mutex_); stop_watch_ptr_->toc("processing_time", true); - pcl::PointCloud::Ptr current_sensor_cloud_ptr(new pcl::PointCloud); - pcl::fromROSMsg(*input, *current_sensor_cloud_ptr); - std::vector radial_ordered_points; + if (!offset_initialized_) { + set_field_offsets(input); + } + std::vector radial_ordered_points; pcl::PointIndices no_ground_indices; - pcl::PointCloud::Ptr no_ground_cloud_ptr(new pcl::PointCloud); - no_ground_cloud_ptr->points.reserve(current_sensor_cloud_ptr->points.size()); if (elevation_grid_mode_) { - convertPointcloudGridScan(current_sensor_cloud_ptr, radial_ordered_points); - classifyPointCloudGridScan(radial_ordered_points, no_ground_indices); + convertPointcloudGridScan(input, radial_ordered_points); + classifyPointCloudGridScan(input, radial_ordered_points, no_ground_indices); } else { - convertPointcloud(current_sensor_cloud_ptr, radial_ordered_points); - classifyPointCloud(radial_ordered_points, no_ground_indices); + convertPointcloud(input, radial_ordered_points); + classifyPointCloud(input, radial_ordered_points, no_ground_indices); } - - extractObjectPoints(current_sensor_cloud_ptr, no_ground_indices, no_ground_cloud_ptr); - - auto no_ground_cloud_msg_ptr = std::make_shared(); - pcl::toROSMsg(*no_ground_cloud_ptr, *no_ground_cloud_msg_ptr); - - no_ground_cloud_msg_ptr->header = input->header; - output = *no_ground_cloud_msg_ptr; - + output.row_step = no_ground_indices.indices.size() * input->point_step; + output.data.resize(output.row_step); + output.width = no_ground_indices.indices.size(); + output.fields.assign(input->fields.begin(), input->fields.begin() + 3); + output.is_dense = true; + output.height = input->height; + output.is_bigendian = input->is_bigendian; + output.point_step = input->point_step; + output.header = input->header; + + extractObjectPoints(input, no_ground_indices, output); if (debug_publisher_ptr_ && stop_watch_ptr_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); @@ -576,20 +627,62 @@ void ScanGroundFilterComponent::filter( } } +// TODO(taisa1): Temporary Implementation: Delete this function definition when all the filter +// nodes conform to new API. +void ScanGroundFilterComponent::filter( + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output) +{ + (void)input; + (void)indices; + (void)output; +} + rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter( const std::vector & p) { + if (get_param(p, "grid_size_m", grid_size_m_)) { + grid_mode_switch_grid_id_ = grid_mode_switch_radius_ / grid_size_m_; + grid_size_rad_ = + normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - + normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); + tan_grid_size_rad_ = std::tan(grid_size_rad_); + RCLCPP_DEBUG(get_logger(), "Setting grid_size_m to: %f.", grid_size_m_); + RCLCPP_DEBUG( + get_logger(), "Setting grid_mode_switch_grid_id to: %f.", grid_mode_switch_grid_id_); + RCLCPP_DEBUG(get_logger(), "Setting grid_size_rad to: %f.", grid_size_rad_); + RCLCPP_DEBUG(get_logger(), "Setting tan_grid_size_rad to: %f.", tan_grid_size_rad_); + } + if (get_param(p, "grid_mode_switch_radius", grid_mode_switch_radius_)) { + grid_mode_switch_grid_id_ = grid_mode_switch_radius_ / grid_size_m_; + grid_mode_switch_angle_rad_ = std::atan2(grid_mode_switch_radius_, virtual_lidar_z_); + grid_size_rad_ = + normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - + normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); + tan_grid_size_rad_ = std::tan(grid_size_rad_); + RCLCPP_DEBUG(get_logger(), "Setting grid_mode_switch_radius to: %f.", grid_mode_switch_radius_); + RCLCPP_DEBUG( + get_logger(), "Setting grid_mode_switch_grid_id to: %f.", grid_mode_switch_grid_id_); + RCLCPP_DEBUG( + get_logger(), "Setting grid_mode_switch_angle_rad to: %f.", grid_mode_switch_angle_rad_); + RCLCPP_DEBUG(get_logger(), "Setting grid_size_rad to: %f.", grid_size_rad_); + RCLCPP_DEBUG(get_logger(), "Setting tan_grid_size_rad to: %f.", tan_grid_size_rad_); + } double global_slope_max_angle_deg{get_parameter("global_slope_max_angle_deg").as_double()}; if (get_param(p, "global_slope_max_angle_deg", global_slope_max_angle_deg)) { global_slope_max_angle_rad_ = deg2rad(global_slope_max_angle_deg); + global_slope_max_ratio_ = std::tan(global_slope_max_angle_rad_); RCLCPP_DEBUG( get_logger(), "Setting global_slope_max_angle_rad to: %f.", global_slope_max_angle_rad_); + RCLCPP_DEBUG(get_logger(), "Setting global_slope_max_ratio to: %f.", global_slope_max_ratio_); } double local_slope_max_angle_deg{get_parameter("local_slope_max_angle_deg").as_double()}; if (get_param(p, "local_slope_max_angle_deg", local_slope_max_angle_deg)) { local_slope_max_angle_rad_ = deg2rad(local_slope_max_angle_deg); + local_slope_max_ratio_ = std::tan(local_slope_max_angle_rad_); RCLCPP_DEBUG( get_logger(), "Setting local_slope_max_angle_rad to: %f.", local_slope_max_angle_rad_); + RCLCPP_DEBUG(get_logger(), "Setting local_slope_max_ratio to: %f.", local_slope_max_ratio_); } double radial_divider_angle_deg{get_parameter("radial_divider_angle_deg").as_double()}; if (get_param(p, "radial_divider_angle_deg", radial_divider_angle_deg)) { @@ -600,9 +693,14 @@ rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter( RCLCPP_DEBUG(get_logger(), "Setting radial_dividers_num to: %zu.", radial_dividers_num_); } if (get_param(p, "split_points_distance_tolerance", split_points_distance_tolerance_)) { + split_points_distance_tolerance_square_ = + split_points_distance_tolerance_ * split_points_distance_tolerance_; RCLCPP_DEBUG( get_logger(), "Setting split_points_distance_tolerance to: %f.", split_points_distance_tolerance_); + RCLCPP_DEBUG( + get_logger(), "Setting split_points_distance_tolerance_square to: %f.", + split_points_distance_tolerance_square_); } if (get_param(p, "split_height_distance", split_height_distance_)) { RCLCPP_DEBUG(get_logger(), "Setting split_height_distance to: %f.", split_height_distance_); diff --git a/perception/ground_segmentation/test/test_scan_ground_filter.cpp b/perception/ground_segmentation/test/test_scan_ground_filter.cpp index 9df9c7e9c7433..444a38ea44754 100644 --- a/perception/ground_segmentation/test/test_scan_ground_filter.cpp +++ b/perception/ground_segmentation/test/test_scan_ground_filter.cpp @@ -131,7 +131,8 @@ class ScanGroundFilterTest : public ::testing::Test // wrapper function to test private function filter void filter(sensor_msgs::msg::PointCloud2 & out_cloud) { - scan_ground_filter_->filter(input_msg_ptr_, nullptr, out_cloud); + pointcloud_preprocessor::TransformInfo transform_info; + scan_ground_filter_->faster_filter(input_msg_ptr_, nullptr, out_cloud, transform_info); } void parse_yaml() diff --git a/perception/image_projection_based_fusion/docs/images/segmentation_pointcloud_fusion.png b/perception/image_projection_based_fusion/docs/images/segmentation_pointcloud_fusion.png new file mode 100644 index 0000000000000..503a4903ab43e Binary files /dev/null and b/perception/image_projection_based_fusion/docs/images/segmentation_pointcloud_fusion.png differ diff --git a/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md index 77188aafb3b22..d59e804f1228c 100644 --- a/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md +++ b/perception/image_projection_based_fusion/docs/segmentation-pointcloud-fusion.md @@ -15,12 +15,12 @@ The node `segmentation_pointcloud_fusion` is a package for filtering pointcloud ### Input -| Name | Type | Description | -| ------------------------ | ----------------------------------- | --------------------------------------------------------- | -| `input` | `sensor_msgs::msg::PointCloud2` | input pointcloud | -| `input/camera_info[0-7]` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes | -| `input/rois[0-7]` | `tier4_perception_msgs::msg::Image` | semantic segmentation mask image | -| `input/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | +| Name | Type | Description | +| ------------------------ | ------------------------------- | -------------------------------------------------------------------------------------- | +| `input` | `sensor_msgs::msg::PointCloud2` | input pointcloud | +| `input/camera_info[0-7]` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes | +| `input/rois[0-7]` | `sensor_msgs::msg::Image` | A gray-scale image of semantic segmentation mask, the pixel value is semantic class id | +| `input/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | ### Output diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index 4457cf3d3992d..a5b41a30be166 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -7,7 +7,7 @@ Yukihiro Saito Shunsuke Miura Yoshi Ri - badai nguyen + Dai Nguyen Kotaro Uetake Tao Zhong Koji Minoda diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt index 7ee5622414366..8fab65ef6b4d7 100644 --- a/perception/lidar_centerpoint/CMakeLists.txt +++ b/perception/lidar_centerpoint/CMakeLists.txt @@ -135,25 +135,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) EXECUTABLE lidar_centerpoint_node ) - ## single inference node ## - ament_auto_add_library(single_inference_lidar_centerpoint_component SHARED - src/single_inference_node.cpp - ) - - target_link_libraries(single_inference_lidar_centerpoint_component - centerpoint_lib - ) - - rclcpp_components_register_node(single_inference_lidar_centerpoint_component - PLUGIN "centerpoint::SingleInferenceLidarCenterPointNode" - EXECUTABLE single_inference_lidar_centerpoint_node - ) - - install(PROGRAMS - scripts/lidar_centerpoint_visualizer.py - DESTINATION lib/${PROJECT_NAME} - ) - ament_export_dependencies(ament_cmake_python) ament_auto_package( diff --git a/perception/lidar_centerpoint/README.md b/perception/lidar_centerpoint/README.md index 2a9adedad9074..ed71349d5bd7f 100644 --- a/perception/lidar_centerpoint/README.md +++ b/perception/lidar_centerpoint/README.md @@ -68,18 +68,6 @@ You can download the onnx format of trained models by clicking on the links belo `Centerpoint` was trained in `nuScenes` (~28k lidar frames) [8] and TIER IV's internal database (~11k lidar frames) for 60 epochs. `Centerpoint tiny` was trained in `Argoverse 2` (~110k lidar frames) [9] and TIER IV's internal database (~11k lidar frames) for 20 epochs. -## Standalone inference and visualization - -In addition to its use as a standard ROS node, `lidar_centerpoint` can also be used to perform inferences in an isolated manner. -To do so, execute the following launcher, where `pcd_path` is the path of the pointcloud to be used for inference. - -```bash -ros2 launch lidar_centerpoint single_inference_lidar_centerpoint.launch.xml pcd_path:=test_pointcloud.pcd detections_path:=test_detections.ply -``` - -`lidar_centerpoint` generates a `ply` file in the provided `detections_path`, which contains the detections as triangle meshes. -These detections can be visualized by most 3D tools, but we also integrate a visualization UI using `Open3D` which is launched alongside `lidar_centerpoint`. - ### Changelog #### v1 (2022/07/06) diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp index 474c9884eb36f..963579e5763c2 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -63,6 +64,8 @@ class LidarCenterPointNode : public rclcpp::Node std::unique_ptr> stop_watch_ptr_{ nullptr}; std::unique_ptr debug_publisher_ptr_{nullptr}; + + std::unique_ptr published_time_publisher_; }; } // namespace centerpoint diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/single_inference_node.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/single_inference_node.hpp deleted file mode 100644 index 45181d55ed46a..0000000000000 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/single_inference_node.hpp +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright 2022 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 LIDAR_CENTERPOINT__SINGLE_INFERENCE_NODE_HPP_ -#define LIDAR_CENTERPOINT__SINGLE_INFERENCE_NODE_HPP_ - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include - -namespace centerpoint -{ - -class SingleInferenceLidarCenterPointNode : public rclcpp::Node -{ -public: - explicit SingleInferenceLidarCenterPointNode(const rclcpp::NodeOptions & node_options); - -private: - void detect(const std::string & pcd_path, const std::string & detections_path); - std::vector getVertices( - const autoware_auto_perception_msgs::msg::Shape & shape, const Eigen::Affine3d & pose) const; - void dumpDetectionsAsMesh( - const autoware_auto_perception_msgs::msg::DetectedObjects & objects_msg, - const std::string & output_path) const; - - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_{tf_buffer_}; - - float score_threshold_{0.0}; - std::vector class_names_; - bool rename_car_to_truck_and_bus_{false}; - bool has_twist_{false}; - - DetectionClassRemapper detection_class_remapper_; - - std::unique_ptr detector_ptr_{nullptr}; -}; - -} // namespace centerpoint - -#endif // LIDAR_CENTERPOINT__SINGLE_INFERENCE_NODE_HPP_ diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/README.md b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/README.md deleted file mode 100644 index 6a743b6e9d9c1..0000000000000 --- a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/README.md +++ /dev/null @@ -1,140 +0,0 @@ -# Run lidar_centerpoint and lidar_centerpoint-tiny simultaneously - -This tutorial is for showing `centerpoint` and `centerpoint_tiny`models’ results simultaneously, making it easier to visualize and compare the performance. - -## Setup Development Environment - -Follow the steps in the Source Installation ([link](https://autowarefoundation.github.io/autoware-documentation/main/installation/autoware/source-installation/)) in Autoware doc. - -If you fail to build autoware environment according to lack of memory, then it is recommended to build autoware sequentially. - -Source the ROS 2 Galactic setup script. - -```bash -source /opt/ros/galactic/setup.bash -``` - -Build the entire autoware repository. - -```bash -colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --parallel-workers=1 -``` - -Or you can use a constrained number of CPU to build only one package. - -```bash -export MAKEFLAGS="-j 4" && MAKE_JOBS=4 colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --parallel-workers 1 --packages-select PACKAGE_NAME -``` - -Source the package. - -```bash -source install/setup.bash -``` - -## Data Preparation - -### Using rosbag dataset - -```bash -ros2 bag play /YOUR/ROSBAG/PATH/ --clock 100 -``` - -Don't forget to add `clock` in order to sync between two rviz display. - -You can also use the sample rosbag provided by autoware [here](https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/rosbag-replay-simulation/). - -If you want to merge several rosbags into one, you can refer to [this tool](https://github.com/jerry73204/rosbag2-merge). - -### Using realtime LiDAR dataset - -Set up your Ethernet connection according to 1.1 - 1.3 in [this website](http://wiki.ros.org/velodyne/Tutorials/Getting%20Started%20with%20the%20Velodyne%20VLP16). - -Download Velodyne ROS driver - -```bash -git clone -b ros2 https://github.com/ros-drivers/velodyne.git -``` - -Source the ROS 2 Galactic setup script. - -```bash -source /opt/ros/galactic/setup.bash -``` - -Compile Velodyne driver - -```bash -cd velodyne -rosdep install -y --from-paths . --ignore-src --rosdistro $ROS_DISTRO -colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -``` - -Edit the configuration file. Specify the LiDAR device IP address in `./velodyne_driver/config/VLP32C-velodyne_driver_node-params.yaml` - -```yaml -velodyne_driver_node: - ros__parameters: - device_ip: 192.168.1.201 //change to your LiDAR device IP address - gps_time: false - time_offset: 0.0 - enabled: true - read_once: false - read_fast: false - repeat_delay: 0.0 - frame_id: velodyne - model: 32C - rpm: 600.0 - port: 2368 -``` - -Launch the velodyne driver. - -```bash -# Terminal 1 -ros2 launch velodyne_driver velodyne_driver_node-VLP32C-launch.py -``` - -Launch the velodyne_pointcloud. - -```bash -# Terminal 2 -ros2 launch velodyne_pointcloud velodyne_convert_node-VLP32C-launch.py -``` - -Point Cloud data will be available on topic `/velodyne_points`. You can check with `ros2 topic echo /velodyne_points`. - -Check [this website](http://wiki.ros.org/velodyne/Tutorials/Getting%20Started%20with%20the%20Velodyne%20VLP16) if there is any unexpected issue. - -## Launch file setting - -Several fields to check in `centerpoint_vs_centerpoint-tiny.launch.xml` before running lidar centerpoint. - -- `input/pointcloud` : set to the topic with input data you want to subscribe. -- `model_path` : set to the path of the model. -- `model_param_path` : set to the path of model's config file. - -## Run CenterPoint and CenterPoint-tiny simultaneously - -Run - -```bash -ros2 launch lidar_centerpoint centerpoint_vs_centerpoint-tiny.launch.xml -``` - -Then you will see two rviz window show immediately. On the left is the result for lidar centerpoint tiny, and on the right is the result for lidar centerpoint. - -![two rviz2 display centerpoint and centerpoint_tiny](https://github.com/autowarefoundation/autoware.universe/assets/58775300/2a89063a-8e0e-4f59-8d48-f339d4f7c0ff) - -## Troubleshooting - -### Bounding Box blink on rviz - -To avoid Bounding Boxes blinking on rviz, you can extend bbox marker lifetime. - -Set `marker_ptr->lifetime` and `marker.lifetime` to a longer lifetime. - -- `marker_ptr->lifetime` are in `PATH/autoware/src/universe/autoware.universe/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp` -- `marker.lifetime` are in `PATH/autoware/src/universe/autoware.universe/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp` - -Make sure to rebuild packages after any change. diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/centerpoint_vs_centerpoint-tiny.launch.xml b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/centerpoint_vs_centerpoint-tiny.launch.xml deleted file mode 100644 index b9c056cfb5686..0000000000000 --- a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/centerpoint_vs_centerpoint-tiny.launch.xml +++ /dev/null @@ -1,44 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz deleted file mode 100644 index 879837e101d47..0000000000000 --- a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz +++ /dev/null @@ -1,1832 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /Sensing1/LiDAR1 - - /Sensing1/LiDAR1/ConcatenatePointCloud1 - - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 - - /Perception1/ObjectRecognition1/Detection1 - - /ConcatenatePointCloud1/Autocompute Value Bounds1 - Splitter Ratio: 0.3668779730796814 - Tree Height: 434 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: ~ - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: AutowareDateTimePanel - Name: AutowareDateTimePanel -Visualization Manager: - Class: "" - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - {} - Update Interval: 0 - Value: false - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/SteeringAngle - Enabled: true - Left: 64 - Length: 128 - Name: SteeringAngle - Scale: 17 - Text Color: 25; 255; 240 - Top: 64 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/steering_status - Value: true - Value Scale: 0.14999249577522278 - Value height offset: 0 - - Class: rviz_plugins/ConsoleMeter - Enabled: true - Left: 256 - Length: 128 - Name: ConsoleMeter - Text Color: 25; 255; 240 - Top: 64 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/velocity_status - Value: true - Value Scale: 0.14999249577522278 - Value height offset: 0 - - Alpha: 0.9990000128746033 - Class: rviz_plugins/VelocityHistory - Color Border Vel Max: 3 - Constant Color: - Color: 255; 255; 255 - Value: true - Enabled: true - Name: VelocityHistory - Scale: 0.30000001192092896 - Timeout: 10 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/velocity_status - Value: true - - Alpha: 0.30000001192092896 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot_description - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - Name: VehicleModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz_plugins/PolarGridDisplay - Color: 255; 255; 255 - Delta Range: 10 - Enabled: true - Max Alpha: 0.5 - Max Range: 100 - Max Wave Alpha: 0.5 - Min Alpha: 0.009999999776482582 - Min Wave Alpha: 0.009999999776482582 - Name: PolarGridDisplay - Reference Frame: base_link - Value: true - Wave Color: 255; 255; 255 - Wave Velocity: 40 - - Class: rviz_plugins/MaxVelocity - Enabled: true - Left: 298 - Length: 48 - Name: MaxVelocity - Text Color: 255; 255; 255 - Top: 140 - Topic: /planning/scenario_planning/current_max_velocity - Value: true - Value Scale: 0.25 - - Class: rviz_plugins/TurnSignal - Enabled: true - Height: 128 - Left: 98 - Name: TurnSignal - Top: 175 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/turn_indicators_status - Value: true - Width: 256 - Enabled: true - Name: Vehicle - Enabled: false - Name: System - - Class: rviz_common/Group - Displays: - - Alpha: 0.20000000298023224 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 28.71826171875 - Min Value: -7.4224700927734375 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 237 - Min Color: 211; 215; 207 - Min Intensity: 0 - Name: PointCloudMap - Position Transformer: XYZ - Selectable: false - Size (Pixels): 1 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/pointcloud_map - Use Fixed Frame: true - Use rainbow: false - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Lanelet2VectorMap - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/vector_map_marker - Value: true - Enabled: false - Name: Map - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.4000000059604645 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 5 - Min Value: -1 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4.560525852145117e-41 - Min Color: 0; 0; 0 - Min Intensity: 7.707141553786494e-44 - Name: ConcatenatePointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 2 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/concatenated/pointcloud - Use Fixed Frame: false - Use rainbow: true - Value: true - - Alpha: 0.9990000128746033 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: MeasurementRange - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/lidar/crop_box_filter/crop_box_polygon - Value: false - Enabled: true - Name: LiDAR - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 233; 185; 110 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: false - Position: - Alpha: 0.20000000298023224 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Head Length: 0.699999988079071 - Head Radius: 1.2000000476837158 - Name: PoseWithCovariance - Shaft Length: 1 - Shaft Radius: 0.5 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/gnss/pose_with_covariance - Value: true - Enabled: false - Name: GNSS - Enabled: false - Name: Sensing - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 0; 170; 255 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: false - Head Length: 0.4000000059604645 - Head Radius: 0.6000000238418579 - Name: PoseWithCovInitial - Shaft Length: 0.6000000238418579 - Shaft Radius: 0.30000001192092896 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/initial_pose_with_covariance - Value: false - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 0; 255; 0 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: false - Head Length: 0.4000000059604645 - Head Radius: 0.6000000238418579 - Name: PoseWithCovAligned - Shaft Length: 0.6000000238418579 - Shaft Radius: 0.30000001192092896 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose_with_covariance - Value: false - - Buffer Size: 200 - Class: rviz_plugins::PoseHistory - Enabled: false - Line: - Alpha: 0.9990000128746033 - Color: 170; 255; 127 - Value: true - Width: 0.10000000149011612 - Name: PoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose - Value: false - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 0; 255; 255 - Color Transformer: "" - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.019999999552965164 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/util/downsample/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 85; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 85; 255; 127 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Aligned - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.019999999552965164 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/points_aligned - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MonteCarloInitialPose - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/monte_carlo_initial_pose_marker - Value: true - Enabled: true - Name: NDT - - Class: rviz_common/Group - Displays: - - Buffer Size: 1000 - Class: rviz_plugins::PoseHistory - Enabled: true - Line: - Alpha: 0.9990000128746033 - Color: 0; 255; 255 - Value: true - Width: 0.10000000149011612 - Name: PoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_twist_fusion_filter/pose - Value: true - Enabled: true - Name: EKF - Enabled: false - Name: Localization - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 15 - Min Value: -2 - Value: false - Axis: Z - Channel Name: z - Class: rviz_default_plugins/PointCloud2 - Color: 200; 200; 200 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 15 - Min Color: 0; 0; 0 - Min Intensity: -5 - Name: NoGroundPointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/obstacle_segmentation/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Segmentation - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/DetectedObjects - Display 3d polygon: true - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: DetectedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/temporary_merged_objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Enabled: true - Name: Detection - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/TrackedObjects - Display 3d polygon: true - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: TrackedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/tracking/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Enabled: false - Name: Tracking - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/PredictedObjects - Display 3d polygon: true - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: false - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: PredictedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Maneuver - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/prediction/maneuver - Value: false - Enabled: true - Name: Prediction - Enabled: true - Name: ObjectRecognition - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: RecognitionResultOnImage - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/traffic_light_recognition/debug/rois - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MapBasedDetectionResult - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers - Value: true - Enabled: true - Name: TrafficLight - - Class: rviz_common/Group - Displays: - - Alpha: 0.5 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/occupancy_grid_map/map - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/occupancy_grid_map/map_updates - Use Timestamp: false - Value: true - Enabled: false - Name: OccupancyGrid - Enabled: false - Name: Perception - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: RouteArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/route_marker - Value: true - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.30000001192092896 - Class: rviz_default_plugins/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.5 - Name: GoalPose - Shaft Length: 3 - Shaft Radius: 0.20000000298023224 - Shape: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/echo_back_goal_pose - Value: true - Enabled: true - Name: MissionPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: true - Name: ScenarioTrajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/trajectory - Value: true - View Path: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Value: true - Width: 2 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: Path - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/path - Value: true - View Path: - Alpha: 0.4000000059604645 - Color: 0; 0; 0 - Constant Color: false - Value: true - Width: 2 - View Velocity: - Alpha: 0.4000000059604645 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/output/path_candidate - Value: true - View Path: - Alpha: 0.30000001192092896 - Color: 115; 210; 22 - Constant Color: true - Value: true - Width: 2 - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (BlindSpot) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/blind_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Crosswalk) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/crosswalk - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (DetectionArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/detection_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Intersection) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/intersection - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (MergeFromPrivateArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/merge_from_private - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (NoStoppingArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_stopping_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (OcclusionSpot) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/occlusion_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (StopLine) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/stop_line - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (TrafficLight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (VirtualTrafficLight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/virtual_traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (RunOut) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/run_out - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Arrow - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Crosswalk - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Blind Spot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: TrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: VirtualTrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: StopLine - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: DetectionArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: OcclusionSpot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/occlusion_spot - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: NoStoppingArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: RunOut - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/run_out - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Avoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: LaneChange - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lanechange - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: LaneFollowing - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lanefollowing - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: PullOver - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/pullover - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: StartPlanner - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/start_planner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: SideShift - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/sideshift - Value: false - Enabled: false - Name: DebugMarker - Enabled: true - Name: BehaviorPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: false - Name: Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/trajectory - Value: false - View Path: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Value: true - Width: 2 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleStop) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (SurroundObstacle) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleAvoidance) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/wall_marker - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: SurroundObstacleCheck - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker - Value: true - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: Footprint - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 239; 41; 41 - Enabled: false - Name: FootprintOffset - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_offset - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 10; 21; 255 - Enabled: false - Name: FootprintRecoverOffset - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_recover_offset - Value: false - Enabled: true - Name: SurroundObstacleChecker - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ObstacleStop - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ObstacleAvoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker - Value: true - Enabled: false - Name: DebugMarker - Enabled: true - Name: MotionPlanning - Enabled: true - Name: LaneDriving - - Class: rviz_common/Group - Displays: - - Alpha: 0.699999988079071 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: false - Name: Costmap - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates - Use Timestamp: false - Value: false - - Alpha: 0.9990000128746033 - Arrow Length: 0.30000001192092896 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray - Color: 255; 25; 0 - Enabled: true - Head Length: 0.10000000149011612 - Head Radius: 0.20000000298023224 - Name: PartialPoseArray - Shaft Length: 0.20000000298023224 - Shaft Radius: 0.05000000074505806 - Shape: Arrow (3D) - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array - Value: true - - Alpha: 0.9990000128746033 - Arrow Length: 0.5 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray - Color: 0; 0; 255 - Enabled: true - Head Length: 0.10000000149011612 - Head Radius: 0.20000000298023224 - Name: PoseArray - Shaft Length: 0.20000000298023224 - Shaft Radius: 0.05000000074505806 - Shape: Arrow (Flat) - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array - Value: true - Enabled: true - Name: Parking - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.30000001192092896 - Class: rviz_default_plugins/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: ModifiedGoal - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/modified_goal - Value: true - Enabled: true - Name: ScenarioPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: PlanningErrorMarker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /planning/planning_diagnostics/planning_error_monitor/debug/marker - Value: true - Enabled: false - Name: Diagnostic - Enabled: false - Name: Planning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: true - Name: Predicted Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/lateral/predicted_trajectory - Value: true - View Path: - Alpha: 1 - Color: 255; 255; 255 - Constant Color: true - Value: true - Width: 0.05000000074505806 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/MPC - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/mpc_follower/debug/markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/PurePursuit - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/pure_pursuit/debug/markers - Value: false - Enabled: false - Name: Control - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Class: autoware_auto_perception_rviz_plugin/DetectedObjects - Display 3d polygon: true - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: true - Line Width: 0.07999999821186066 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Name: centerpoint - Namespaces: - label: true - shape: true - twist: true - velocity: true - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /centerpoint/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - - Class: rviz_plugins/PolarGridDisplay - Color: 255; 255; 255 - Delta Range: 10 - Enabled: true - Max Alpha: 0.5 - Max Range: 100 - Max Wave Alpha: 0.5 - Min Alpha: 0.009999999776482582 - Min Wave Alpha: 0.009999999776482582 - Name: PolarGridDisplay - Reference Frame: base_link - Value: true - Wave Color: 255; 255; 255 - Wave Velocity: 40 - - Alpha: 0.4000000059604645 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 5 - Min Value: -1 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4.560525852145117e-41 - Min Color: 0; 0; 0 - Min Intensity: 7.707141553786494e-44 - Name: ConcatenatePointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 2 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/concatenated/pointcloud - Use Fixed Frame: false - Use rainbow: true - Value: true - Enabled: true - Global Options: - Background Color: 10; 10; 10 - Fixed Frame: base_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/goal - - Acceleration: 0 - Class: rviz_plugins/PedestrianInitialPoseTool - Interactive: false - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 1 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/CarInitialPoseTool - H vehicle height: 2 - Interactive: false - L vehicle length: 4 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 3 - W vehicle width: 1.7999999523162842 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/BusInitialPoseTool - H vehicle height: 3.5 - Interactive: false - L vehicle length: 10.5 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - W vehicle width: 2.5 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Class: rviz_plugins/MissionCheckpointTool - Pose Topic: /planning/mission_planning/checkpoint - Theta std deviation: 0.2617993950843811 - X std deviation: 0.5 - Y std deviation: 0.5 - Z position: 0 - - Class: rviz_plugins/DeleteAllObjectsTool - Pose Topic: /simulation/dummy_perception_publisher/object_info - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/XYOrbit - Distance: 50.32217788696289 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6553983092308044 - Target Frame: base_link - Value: XYOrbit (rviz_default_plugins) - Yaw: 3.20540452003479 - Saved: - - Class: rviz_default_plugins/ThirdPersonFollower - Distance: 18 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: ThirdPersonFollower - Near Clip Distance: 0.009999999776482582 - Pitch: 0.20000000298023224 - Target Frame: base_link - Value: ThirdPersonFollower (rviz) - Yaw: 3.141592025756836 - - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: TopDownOrtho - Near Clip Distance: 0.009999999776482582 - Scale: 10 - Target Frame: viewer - Value: TopDownOrtho (rviz) - X: 0 - Y: 0 -Window Geometry: - AutowareDateTimePanel: - collapsed: true - Displays: - collapsed: true - Height: 1028 - Hide Left Dock: true - Hide Right Dock: false - QMainWindow State: 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 - RecognitionResultOnImage: - collapsed: false - Selection: - collapsed: false - Tool Properties: - collapsed: true - Views: - collapsed: true - Width: 928 - X: 992 - Y: 24 diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz deleted file mode 100644 index 539f89305e3c6..0000000000000 --- a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz +++ /dev/null @@ -1,1833 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /Sensing1/LiDAR1 - - /Sensing1/LiDAR1/ConcatenatePointCloud1 - - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 - - /Perception1/ObjectRecognition1/Detection1 - - /centerpoint_tiny1 - - /ConcatenatePointCloud1/Autocompute Value Bounds1 - Splitter Ratio: 0.3668779730796814 - Tree Height: 451 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: ~ - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: AutowareDateTimePanel - Name: AutowareDateTimePanel -Visualization Manager: - Class: "" - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - {} - Update Interval: 0 - Value: false - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/SteeringAngle - Enabled: true - Left: 64 - Length: 128 - Name: SteeringAngle - Scale: 17 - Text Color: 25; 255; 240 - Top: 64 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/steering_status - Value: true - Value Scale: 0.14999249577522278 - Value height offset: 0 - - Class: rviz_plugins/ConsoleMeter - Enabled: true - Left: 256 - Length: 128 - Name: ConsoleMeter - Text Color: 25; 255; 240 - Top: 64 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/velocity_status - Value: true - Value Scale: 0.14999249577522278 - Value height offset: 0 - - Alpha: 0.9990000128746033 - Class: rviz_plugins/VelocityHistory - Color Border Vel Max: 3 - Constant Color: - Color: 255; 255; 255 - Value: true - Enabled: true - Name: VelocityHistory - Scale: 0.30000001192092896 - Timeout: 10 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/velocity_status - Value: true - - Alpha: 0.30000001192092896 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot_description - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - Name: VehicleModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz_plugins/PolarGridDisplay - Color: 255; 255; 255 - Delta Range: 10 - Enabled: true - Max Alpha: 0.5 - Max Range: 100 - Max Wave Alpha: 0.5 - Min Alpha: 0.009999999776482582 - Min Wave Alpha: 0.009999999776482582 - Name: PolarGridDisplay - Reference Frame: base_link - Value: true - Wave Color: 255; 255; 255 - Wave Velocity: 40 - - Class: rviz_plugins/MaxVelocity - Enabled: true - Left: 298 - Length: 48 - Name: MaxVelocity - Text Color: 255; 255; 255 - Top: 140 - Topic: /planning/scenario_planning/current_max_velocity - Value: true - Value Scale: 0.25 - - Class: rviz_plugins/TurnSignal - Enabled: true - Height: 128 - Left: 98 - Name: TurnSignal - Top: 175 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/turn_indicators_status - Value: true - Width: 256 - Enabled: true - Name: Vehicle - Enabled: false - Name: System - - Class: rviz_common/Group - Displays: - - Alpha: 0.20000000298023224 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 28.71826171875 - Min Value: -7.4224700927734375 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 237 - Min Color: 211; 215; 207 - Min Intensity: 0 - Name: PointCloudMap - Position Transformer: XYZ - Selectable: false - Size (Pixels): 1 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/pointcloud_map - Use Fixed Frame: true - Use rainbow: false - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Lanelet2VectorMap - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/vector_map_marker - Value: true - Enabled: false - Name: Map - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.4000000059604645 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 5 - Min Value: -1 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4.560525852145117e-41 - Min Color: 0; 0; 0 - Min Intensity: 7.707141553786494e-44 - Name: ConcatenatePointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 2 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/concatenated/pointcloud - Use Fixed Frame: false - Use rainbow: true - Value: true - - Alpha: 0.9990000128746033 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: MeasurementRange - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/lidar/crop_box_filter/crop_box_polygon - Value: false - Enabled: true - Name: LiDAR - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 233; 185; 110 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: false - Position: - Alpha: 0.20000000298023224 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Head Length: 0.699999988079071 - Head Radius: 1.2000000476837158 - Name: PoseWithCovariance - Shaft Length: 1 - Shaft Radius: 0.5 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/gnss/pose_with_covariance - Value: true - Enabled: false - Name: GNSS - Enabled: false - Name: Sensing - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 0; 170; 255 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: false - Head Length: 0.4000000059604645 - Head Radius: 0.6000000238418579 - Name: PoseWithCovInitial - Shaft Length: 0.6000000238418579 - Shaft Radius: 0.30000001192092896 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/initial_pose_with_covariance - Value: false - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 0; 255; 0 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: false - Head Length: 0.4000000059604645 - Head Radius: 0.6000000238418579 - Name: PoseWithCovAligned - Shaft Length: 0.6000000238418579 - Shaft Radius: 0.30000001192092896 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose_with_covariance - Value: false - - Buffer Size: 200 - Class: rviz_plugins::PoseHistory - Enabled: false - Line: - Alpha: 0.9990000128746033 - Color: 170; 255; 127 - Value: true - Width: 0.10000000149011612 - Name: PoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose - Value: false - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 0; 255; 255 - Color Transformer: "" - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.019999999552965164 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/util/downsample/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 85; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 85; 255; 127 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Aligned - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.019999999552965164 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/points_aligned - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MonteCarloInitialPose - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/monte_carlo_initial_pose_marker - Value: true - Enabled: true - Name: NDT - - Class: rviz_common/Group - Displays: - - Buffer Size: 1000 - Class: rviz_plugins::PoseHistory - Enabled: true - Line: - Alpha: 0.9990000128746033 - Color: 0; 255; 255 - Value: true - Width: 0.10000000149011612 - Name: PoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_twist_fusion_filter/pose - Value: true - Enabled: true - Name: EKF - Enabled: false - Name: Localization - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 15 - Min Value: -2 - Value: false - Axis: Z - Channel Name: z - Class: rviz_default_plugins/PointCloud2 - Color: 200; 200; 200 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 15 - Min Color: 0; 0; 0 - Min Intensity: -5 - Name: NoGroundPointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/obstacle_segmentation/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Segmentation - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/DetectedObjects - Display 3d polygon: true - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: DetectedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/temporary_merged_objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Enabled: true - Name: Detection - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/TrackedObjects - Display 3d polygon: true - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: TrackedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/tracking/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Enabled: false - Name: Tracking - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/PredictedObjects - Display 3d polygon: true - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: false - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: PredictedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Maneuver - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/prediction/maneuver - Value: false - Enabled: true - Name: Prediction - Enabled: true - Name: ObjectRecognition - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: RecognitionResultOnImage - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/traffic_light_recognition/debug/rois - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MapBasedDetectionResult - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers - Value: true - Enabled: true - Name: TrafficLight - - Class: rviz_common/Group - Displays: - - Alpha: 0.5 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/occupancy_grid_map/map - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/occupancy_grid_map/map_updates - Use Timestamp: false - Value: true - Enabled: false - Name: OccupancyGrid - Enabled: false - Name: Perception - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: RouteArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/route_marker - Value: true - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.30000001192092896 - Class: rviz_default_plugins/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.5 - Name: GoalPose - Shaft Length: 3 - Shaft Radius: 0.20000000298023224 - Shape: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/echo_back_goal_pose - Value: true - Enabled: true - Name: MissionPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: true - Name: ScenarioTrajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/trajectory - Value: true - View Path: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Value: true - Width: 2 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: Path - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/path - Value: true - View Path: - Alpha: 0.4000000059604645 - Color: 0; 0; 0 - Constant Color: false - Value: true - Width: 2 - View Velocity: - Alpha: 0.4000000059604645 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/output/path_candidate - Value: true - View Path: - Alpha: 0.30000001192092896 - Color: 115; 210; 22 - Constant Color: true - Value: true - Width: 2 - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (BlindSpot) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/blind_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Crosswalk) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/crosswalk - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (DetectionArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/detection_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Intersection) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/intersection - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (MergeFromPrivateArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/merge_from_private - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (NoStoppingArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_stopping_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (OcclusionSpot) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/occlusion_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (StopLine) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/stop_line - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (TrafficLight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (VirtualTrafficLight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/virtual_traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (RunOut) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/run_out - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Arrow - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Crosswalk - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Blind Spot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: TrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: VirtualTrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: StopLine - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: DetectionArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: OcclusionSpot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/occlusion_spot - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: NoStoppingArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: RunOut - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/run_out - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Avoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: LaneChange - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lanechange - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: LaneFollowing - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lanefollowing - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: PullOver - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/pullover - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: StartPlanner - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/start_planner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: SideShift - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/sideshift - Value: false - Enabled: false - Name: DebugMarker - Enabled: true - Name: BehaviorPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: false - Name: Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/trajectory - Value: false - View Path: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Value: true - Width: 2 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleStop) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (SurroundObstacle) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleAvoidance) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/wall_marker - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: SurroundObstacleCheck - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker - Value: true - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: Footprint - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 239; 41; 41 - Enabled: false - Name: FootprintOffset - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_offset - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 10; 21; 255 - Enabled: false - Name: FootprintRecoverOffset - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_recover_offset - Value: false - Enabled: true - Name: SurroundObstacleChecker - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ObstacleStop - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ObstacleAvoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker - Value: true - Enabled: false - Name: DebugMarker - Enabled: true - Name: MotionPlanning - Enabled: true - Name: LaneDriving - - Class: rviz_common/Group - Displays: - - Alpha: 0.699999988079071 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: false - Name: Costmap - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates - Use Timestamp: false - Value: false - - Alpha: 0.9990000128746033 - Arrow Length: 0.30000001192092896 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray - Color: 255; 25; 0 - Enabled: true - Head Length: 0.10000000149011612 - Head Radius: 0.20000000298023224 - Name: PartialPoseArray - Shaft Length: 0.20000000298023224 - Shaft Radius: 0.05000000074505806 - Shape: Arrow (3D) - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array - Value: true - - Alpha: 0.9990000128746033 - Arrow Length: 0.5 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray - Color: 0; 0; 255 - Enabled: true - Head Length: 0.10000000149011612 - Head Radius: 0.20000000298023224 - Name: PoseArray - Shaft Length: 0.20000000298023224 - Shaft Radius: 0.05000000074505806 - Shape: Arrow (Flat) - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array - Value: true - Enabled: true - Name: Parking - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.30000001192092896 - Class: rviz_default_plugins/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: ModifiedGoal - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/modified_goal - Value: true - Enabled: true - Name: ScenarioPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: PlanningErrorMarker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /planning/planning_diagnostics/planning_error_monitor/debug/marker - Value: true - Enabled: false - Name: Diagnostic - Enabled: false - Name: Planning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: true - Name: Predicted Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/lateral/predicted_trajectory - Value: true - View Path: - Alpha: 1 - Color: 255; 255; 255 - Constant Color: true - Value: true - Width: 0.05000000074505806 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/MPC - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/mpc_follower/debug/markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/PurePursuit - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/pure_pursuit/debug/markers - Value: false - Enabled: false - Name: Control - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Class: autoware_auto_perception_rviz_plugin/DetectedObjects - Display 3d polygon: true - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: true - Line Width: 0.07999999821186066 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Name: centerpoint_tiny - Namespaces: - label: true - shape: true - twist: true - velocity: true - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /centerpoint_tiny/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - - Class: rviz_plugins/PolarGridDisplay - Color: 255; 255; 255 - Delta Range: 10 - Enabled: true - Max Alpha: 0.5 - Max Range: 100 - Max Wave Alpha: 0.5 - Min Alpha: 0.009999999776482582 - Min Wave Alpha: 0.009999999776482582 - Name: PolarGridDisplay - Reference Frame: base_link - Value: true - Wave Color: 255; 255; 255 - Wave Velocity: 40 - - Alpha: 0.4000000059604645 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 5 - Min Value: -1 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4.560525852145117e-41 - Min Color: 0; 0; 0 - Min Intensity: 7.707141553786494e-44 - Name: ConcatenatePointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 2 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/concatenated/pointcloud - Use Fixed Frame: false - Use rainbow: true - Value: true - Enabled: true - Global Options: - Background Color: 10; 10; 10 - Fixed Frame: base_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/goal - - Acceleration: 0 - Class: rviz_plugins/PedestrianInitialPoseTool - Interactive: false - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 1 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/CarInitialPoseTool - H vehicle height: 2 - Interactive: false - L vehicle length: 4 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 3 - W vehicle width: 1.7999999523162842 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/BusInitialPoseTool - H vehicle height: 3.5 - Interactive: false - L vehicle length: 10.5 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - W vehicle width: 2.5 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Class: rviz_plugins/MissionCheckpointTool - Pose Topic: /planning/mission_planning/checkpoint - Theta std deviation: 0.2617993950843811 - X std deviation: 0.5 - Y std deviation: 0.5 - Z position: 0 - - Class: rviz_plugins/DeleteAllObjectsTool - Pose Topic: /simulation/dummy_perception_publisher/object_info - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/XYOrbit - Distance: 50.32217788696289 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.6553983092308044 - Target Frame: base_link - Value: XYOrbit (rviz_default_plugins) - Yaw: 3.20540452003479 - Saved: - - Class: rviz_default_plugins/ThirdPersonFollower - Distance: 18 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: ThirdPersonFollower - Near Clip Distance: 0.009999999776482582 - Pitch: 0.20000000298023224 - Target Frame: base_link - Value: ThirdPersonFollower (rviz) - Yaw: 3.141592025756836 - - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: TopDownOrtho - Near Clip Distance: 0.009999999776482582 - Scale: 10 - Target Frame: viewer - Value: TopDownOrtho (rviz) - X: 0 - Y: 0 -Window Geometry: - AutowareDateTimePanel: - collapsed: true - Displays: - collapsed: true - Height: 1028 - Hide Left Dock: true - Hide Right Dock: false - QMainWindow State: 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 - RecognitionResultOnImage: - collapsed: false - Selection: - collapsed: false - Tool Properties: - collapsed: true - Views: - collapsed: true - Width: 928 - X: 65 - Y: 24 diff --git a/perception/lidar_centerpoint/launch/single_inference_lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/single_inference_lidar_centerpoint.launch.xml deleted file mode 100644 index 491abfbad7764..0000000000000 --- a/perception/lidar_centerpoint/launch/single_inference_lidar_centerpoint.launch.xml +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/perception/lidar_centerpoint/scripts/lidar_centerpoint_visualizer.py b/perception/lidar_centerpoint/scripts/lidar_centerpoint_visualizer.py deleted file mode 100755 index 5a1135379615f..0000000000000 --- a/perception/lidar_centerpoint/scripts/lidar_centerpoint_visualizer.py +++ /dev/null @@ -1,55 +0,0 @@ -#!/usr/bin/env python -# Copyright 2022 TIER IV, Inc. All rights reserved. -# -# 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. - -import os -import time - -import open3d as o3d -import rclpy -from rclpy.node import Node - - -def main(args=None): - rclpy.init(args=args) - - node = Node("lidar_centerpoint_visualizer") - node.declare_parameter("pcd_path", rclpy.Parameter.Type.STRING) - node.declare_parameter("detections_path", rclpy.Parameter.Type.STRING) - - pcd_path = node.get_parameter("pcd_path").get_parameter_value().string_value - detections_path = node.get_parameter("detections_path").get_parameter_value().string_value - - while not os.path.exists(pcd_path) and not os.path.exists(detections_path): - time.sleep(1.0) - - if not rclpy.ok(): - rclpy.shutdown() - return - - mesh = o3d.io.read_triangle_mesh(detections_path) - pcd = o3d.io.read_point_cloud(pcd_path) - - mesh_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=1.0, origin=[0, 0, 0]) - - detection_lines = o3d.geometry.LineSet.create_from_triangle_mesh(mesh) - detection_lines.paint_uniform_color([1.0, 0.0, 1.0]) - - o3d.visualization.draw_geometries([mesh_frame, pcd, detection_lines]) - - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index d370a60a26aa5..65bdafe94fe7d 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -126,6 +126,7 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); rclcpp::shutdown(); } + published_time_publisher_ = std::make_unique(this); } void LidarCenterPointNode::pointCloudCallback( @@ -163,6 +164,7 @@ void LidarCenterPointNode::pointCloudCallback( if (objects_sub_count > 0) { objects_pub_->publish(output_msg); + published_time_publisher_->publish_if_subscribed(objects_pub_, output_msg.header.stamp); } // add processing time for debug diff --git a/perception/lidar_centerpoint/src/single_inference_node.cpp b/perception/lidar_centerpoint/src/single_inference_node.cpp deleted file mode 100644 index 77099ad8226a2..0000000000000 --- a/perception/lidar_centerpoint/src/single_inference_node.cpp +++ /dev/null @@ -1,248 +0,0 @@ -// Copyright 2022 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 "lidar_centerpoint/single_inference_node.hpp" - -#include -#include -#include -#include -#include - -#include - -#include -#include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include -#include -#include - -namespace centerpoint -{ -SingleInferenceLidarCenterPointNode::SingleInferenceLidarCenterPointNode( - const rclcpp::NodeOptions & node_options) -: Node("lidar_center_point", node_options), tf_buffer_(this->get_clock()) -{ - const float score_threshold = - static_cast(this->declare_parameter("score_threshold", 0.35)); - const float circle_nms_dist_threshold = - static_cast(this->declare_parameter("circle_nms_dist_threshold", 1.5)); - const auto yaw_norm_thresholds = - this->declare_parameter>("yaw_norm_thresholds"); - const std::string densification_world_frame_id = - this->declare_parameter("densification_world_frame_id", "map"); - const int densification_num_past_frames = - this->declare_parameter("densification_num_past_frames", 1); - const std::string trt_precision = this->declare_parameter("trt_precision", "fp16"); - const std::string encoder_onnx_path = this->declare_parameter("encoder_onnx_path"); - const std::string encoder_engine_path = - this->declare_parameter("encoder_engine_path"); - const std::string head_onnx_path = this->declare_parameter("head_onnx_path"); - const std::string head_engine_path = this->declare_parameter("head_engine_path"); - class_names_ = this->declare_parameter>("class_names"); - has_twist_ = this->declare_parameter("has_twist", false); - const std::size_t point_feature_size = - static_cast(this->declare_parameter("point_feature_size")); - const std::size_t max_voxel_size = - static_cast(this->declare_parameter("max_voxel_size")); - const auto point_cloud_range = this->declare_parameter>("point_cloud_range"); - const auto voxel_size = this->declare_parameter>("voxel_size"); - const std::size_t downsample_factor = - static_cast(this->declare_parameter("downsample_factor")); - const std::size_t encoder_in_feature_size = - static_cast(this->declare_parameter("encoder_in_feature_size")); - const auto allow_remapping_by_area_matrix = - this->declare_parameter>("allow_remapping_by_area_matrix"); - const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); - const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); - const auto pcd_path = this->declare_parameter("pcd_path"); - const auto detections_path = this->declare_parameter("detections_path"); - - detection_class_remapper_.setParameters( - allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); - - NetworkParam encoder_param(encoder_onnx_path, encoder_engine_path, trt_precision); - NetworkParam head_param(head_onnx_path, head_engine_path, trt_precision); - DensificationParam densification_param( - densification_world_frame_id, densification_num_past_frames); - - if (point_cloud_range.size() != 6) { - RCLCPP_WARN_STREAM( - rclcpp::get_logger("single_inference_lidar_centerpoint"), - "The size of point_cloud_range != 6: use the default parameters."); - } - if (voxel_size.size() != 3) { - RCLCPP_WARN_STREAM( - rclcpp::get_logger("single_inference_lidar_centerpoint"), - "The size of voxel_size != 3: use the default parameters."); - } - CenterPointConfig config( - class_names_.size(), point_feature_size, max_voxel_size, point_cloud_range, voxel_size, - downsample_factor, encoder_in_feature_size, score_threshold, circle_nms_dist_threshold, - yaw_norm_thresholds); - detector_ptr_ = - std::make_unique(encoder_param, head_param, densification_param, config); - - detect(pcd_path, detections_path); - exit(0); -} - -std::vector SingleInferenceLidarCenterPointNode::getVertices( - const autoware_auto_perception_msgs::msg::Shape & shape, const Eigen::Affine3d & pose) const -{ - std::vector vertices; - Eigen::Vector3d vertex; - - vertex.x() = -shape.dimensions.x / 2.0; - vertex.y() = -shape.dimensions.y / 2.0; - vertex.z() = shape.dimensions.z / 2.0; - vertices.push_back(pose * vertex); - - vertex.x() = -shape.dimensions.x / 2.0; - vertex.y() = shape.dimensions.y / 2.0; - vertex.z() = shape.dimensions.z / 2.0; - vertices.push_back(pose * vertex); - - vertex.x() = -shape.dimensions.x / 2.0; - vertex.y() = shape.dimensions.y / 2.0; - vertex.z() = -shape.dimensions.z / 2.0; - vertices.push_back(pose * vertex); - - vertex.x() = shape.dimensions.x / 2.0; - vertex.y() = shape.dimensions.y / 2.0; - vertex.z() = shape.dimensions.z / 2.0; - vertices.push_back(pose * vertex); - - vertex.x() = shape.dimensions.x / 2.0; - vertex.y() = shape.dimensions.y / 2.0; - vertex.z() = -shape.dimensions.z / 2.0; - vertices.push_back(pose * vertex); - - vertex.x() = shape.dimensions.x / 2.0; - vertex.y() = -shape.dimensions.y / 2.0; - vertex.z() = shape.dimensions.z / 2.0; - vertices.push_back(pose * vertex); - - vertex.x() = shape.dimensions.x / 2.0; - vertex.y() = -shape.dimensions.y / 2.0; - vertex.z() = -shape.dimensions.z / 2.0; - vertices.push_back(pose * vertex); - - vertex.x() = -shape.dimensions.x / 2.0; - vertex.y() = -shape.dimensions.y / 2.0; - vertex.z() = -shape.dimensions.z / 2.0; - vertices.push_back(pose * vertex); - - return vertices; -} - -void SingleInferenceLidarCenterPointNode::detect( - const std::string & pcd_path, const std::string & detections_path) -{ - sensor_msgs::msg::PointCloud2 msg; - pcl::PointCloud::Ptr pc_ptr(new pcl::PointCloud()); - - pcl::io::loadPCDFile(pcd_path, *pc_ptr); - pcl::toROSMsg(*pc_ptr, msg); - msg.header.frame_id = "lidar_frame"; - - std::vector det_boxes3d; - bool is_success = detector_ptr_->detect(msg, tf_buffer_, det_boxes3d); - if (!is_success) { - return; - } - - autoware_auto_perception_msgs::msg::DetectedObjects output_msg; - output_msg.header = msg.header; - for (const auto & box3d : det_boxes3d) { - autoware_auto_perception_msgs::msg::DetectedObject obj; - box3DToDetectedObject(box3d, class_names_, has_twist_, obj); - output_msg.objects.emplace_back(obj); - } - - detection_class_remapper_.mapClasses(output_msg); - - dumpDetectionsAsMesh(output_msg, detections_path); - - RCLCPP_INFO( - rclcpp::get_logger("single_inference_lidar_centerpoint"), - "The detection results were saved as meshes in %s", detections_path.c_str()); -} - -void SingleInferenceLidarCenterPointNode::dumpDetectionsAsMesh( - const autoware_auto_perception_msgs::msg::DetectedObjects & objects_msg, - const std::string & output_path) const -{ - std::ofstream ofs(output_path, std::ofstream::out); - std::stringstream vertices_stream; - std::stringstream faces_stream; - int index = 0; - int num_detections = static_cast(objects_msg.objects.size()); - - ofs << "ply" << std::endl; - ofs << "format ascii 1.0" << std::endl; - ofs << "comment created by lidar_centerpoint" << std::endl; - ofs << "element vertex " << 8 * num_detections << std::endl; - ofs << "property float x" << std::endl; - ofs << "property float y" << std::endl; - ofs << "property float z" << std::endl; - ofs << "element face " << 12 * num_detections << std::endl; - ofs << "property list uchar uint vertex_indices" << std::endl; - ofs << "end_header" << std::endl; - - auto streamFace = [&faces_stream](int v1, int v2, int v3) { - faces_stream << std::to_string(3) << " " << std::to_string(v1) << " " << std::to_string(v2) - << " " << std::to_string(v3) << std::endl; - }; - - for (const auto & object : objects_msg.objects) { - Eigen::Affine3d pose_affine; - tf2::fromMsg(object.kinematics.pose_with_covariance.pose, pose_affine); - - std::vector vertices = getVertices(object.shape, pose_affine); - - for (const auto & vertex : vertices) { - vertices_stream << vertex.x() << " " << vertex.y() << " " << vertex.z() << std::endl; - } - - streamFace(index + 1, index + 3, index + 4); - streamFace(index + 3, index + 5, index + 6); - streamFace(index + 0, index + 7, index + 5); - streamFace(index + 7, index + 2, index + 4); - streamFace(index + 5, index + 3, index + 1); - streamFace(index + 7, index + 0, index + 2); - streamFace(index + 2, index + 1, index + 4); - streamFace(index + 4, index + 3, index + 6); - streamFace(index + 5, index + 7, index + 6); - streamFace(index + 6, index + 7, index + 4); - streamFace(index + 0, index + 5, index + 1); - index += 8; - } - - ofs << vertices_stream.str(); - ofs << faces_stream.str(); - - ofs.close(); -} - -} // namespace centerpoint - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(centerpoint::SingleInferenceLidarCenterPointNode) diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index a58e19db70304..7989a5fbf6af9 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -218,11 +218,13 @@ If the target object is inside the road or crosswalk, this module outputs one or ### Output -| Name | Type | Description | -| ------------------------ | ------------------------------------------------------ | ------------------------------------------------------------------------------------- | -| `~/input/objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking objects. Default is set to `/perception/object_recognition/tracking/objects` | -| `~/output/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | tracking objects with predicted path. | -| `~/objects_path_markers` | `visualization_msgs::msg::MarkerArray` | marker for visualization. | +| Name | Type | Description | +| ---------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------------------------- | +| `~/input/objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking objects. Default is set to `/perception/object_recognition/tracking/objects` | +| `~/output/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | tracking objects with predicted path. | +| `~/objects_path_markers` | `visualization_msgs::msg::MarkerArray` | marker for visualization. | +| `~/debug/processing_time_ms` | `std_msgs::msg::Float64` | processing time of this module. | +| `~/debug/cyclic_time_ms` | `std_msgs::msg::Float64` | cyclic time of this module. | ## Parameters diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 321342c3d8367..829c4d6e4a114 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -21,6 +21,8 @@ #include "tier4_autoware_utils/ros/update_param.hpp" #include +#include +#include #include #include #include @@ -33,7 +35,6 @@ #include #include #include -#include #include #include @@ -125,11 +126,14 @@ class MapBasedPredictionNode : public rclcpp::Node // ROS Publisher and Subscriber rclcpp::Publisher::SharedPtr pub_objects_; rclcpp::Publisher::SharedPtr pub_debug_markers_; - rclcpp::Publisher::SharedPtr pub_calculation_time_; rclcpp::Subscription::SharedPtr sub_objects_; rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::SharedPtr sub_traffic_signals_; + // debug publisher + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; + // Object History std::unordered_map> objects_history_; std::map, rclcpp::Time> stopped_times_against_green_; @@ -196,8 +200,7 @@ class MapBasedPredictionNode : public rclcpp::Node std::vector distance_set_for_no_intention_to_walk_; std::vector timeout_set_for_no_intention_to_walk_; - // Stop watch - StopWatch stop_watch_; + std::unique_ptr published_time_publisher_; // Member Functions void mapCallback(const HADMapBin::ConstSharedPtr msg); diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 21697bb1b5f1a..e12b7c54c3a9f 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -646,14 +646,6 @@ ObjectClassification::_label_type changeLabelForPrediction( } } -StringStamped createStringStamped(const rclcpp::Time & now, const double data) -{ - StringStamped msg; - msg.stamp = now; - msg.data = std::to_string(data); - return msg; -} - // NOTE: These two functions are copied from the route_handler package. lanelet::Lanelets getRightOppositeLanelets( const std::shared_ptr & lanelet_map_ptr, @@ -816,10 +808,16 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ pub_objects_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); pub_debug_markers_ = this->create_publisher("maneuver", rclcpp::QoS{1}); - pub_calculation_time_ = create_publisher("~/debug/calculation_time", 1); + processing_time_publisher_ = + std::make_unique(this, "map_based_prediction"); + published_time_publisher_ = std::make_unique(this); set_param_res_ = this->add_on_set_parameters_callback( std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1)); + + stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); } rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam( @@ -893,7 +891,7 @@ void MapBasedPredictionNode::trafficSignalsCallback(const TrafficSignalArray::Co void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects) { - stop_watch_.tic(); + stop_watch_ptr_->toc("processing_time", true); // Guard for map pointer and frame transformation if (!lanelet_map_ptr_) { return; @@ -1115,9 +1113,14 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt // Publish Results pub_objects_->publish(output); + published_time_publisher_->publish_if_subscribed(pub_objects_, output.header.stamp); pub_debug_markers_->publish(debug_markers); - const auto calculation_time_msg = createStringStamped(now(), stop_watch_.toc()); - pub_calculation_time_->publish(calculation_time_msg); + const auto processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + processing_time_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + processing_time_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); } bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path) diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index f5fbc8ff950e8..fe90be8c583c2 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -22,6 +22,13 @@ include_directories( # Generate exe file set(MULTI_OBJECT_TRACKER_SRC src/multi_object_tracker_core.cpp + src/data_association/data_association.cpp + src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp + src/tracker/motion_model/motion_model_base.cpp + src/tracker/motion_model/bicycle_motion_model.cpp + # cspell: ignore ctrv + src/tracker/motion_model/ctrv_motion_model.cpp + src/tracker/motion_model/cv_motion_model.cpp src/tracker/model/tracker_base.cpp src/tracker/model/big_vehicle_tracker.cpp src/tracker/model/normal_vehicle_tracker.cpp @@ -31,8 +38,6 @@ set(MULTI_OBJECT_TRACKER_SRC src/tracker/model/pedestrian_and_bicycle_tracker.cpp src/tracker/model/unknown_tracker.cpp src/tracker/model/pass_through_tracker.cpp - src/data_association/data_association.cpp - src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp ) ament_auto_add_library(multi_object_tracker_node SHARED diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index 95d33b78ff184..ca59f994a3b0e 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -107,6 +108,8 @@ class MultiObjectTracker : public rclcpp::Node std::map tracker_map_; + std::unique_ptr published_time_publisher_; + void onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); void onTimer(); @@ -124,7 +127,7 @@ class MultiObjectTracker : public rclcpp::Node const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) const; - void publish(const rclcpp::Time & time); + void publish(const rclcpp::Time & time) const; inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; }; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index ba50933eddaec..2d69334a2f43f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -20,6 +20,7 @@ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ #include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" #include @@ -30,36 +31,15 @@ class BicycleTracker : public Tracker rclcpp::Logger logger_; private: - KalmanFilter ekf_; - rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { - char dim_x = 5; - float q_stddev_acc_long; - float q_stddev_acc_lat; - float q_stddev_yaw_rate_min; - float q_stddev_yaw_rate_max; - float q_cov_slip_rate_min; - float q_cov_slip_rate_max; - float q_max_slip_angle; - float p0_cov_vel; - float p0_cov_slip; - float r_cov_x; - float r_cov_y; - float r_cov_yaw; - float p0_cov_x; - float p0_cov_y; - float p0_cov_yaw; + double r_cov_x; + double r_cov_y; + double r_cov_yaw; } ekf_params_; - double max_vel_; - double max_slip_; - double lf_; - double lr_; - float z_; + double z_; -private: struct BoundingBox { double length; @@ -67,7 +47,11 @@ class BicycleTracker : public Tracker double height; }; BoundingBox bounding_box_; - BoundingBox last_input_bounding_box_; + +private: + BicycleMotionModel motion_model_; + const char DIM = motion_model_.DIM; + using IDX = BicycleMotionModel::IDX; public: BicycleTracker( @@ -75,10 +59,12 @@ class BicycleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; - bool predict(const double dt, KalmanFilter & ekf) const; bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index e19b6382ad952..8d49138b94a24 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -20,47 +20,28 @@ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ #include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" #include + class BigVehicleTracker : public Tracker { private: autoware_auto_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; - int last_nearest_corner_index_; private: - KalmanFilter ekf_; - rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { - char dim_x = 5; - float q_stddev_acc_long; - float q_stddev_acc_lat; - float q_stddev_yaw_rate_min; - float q_stddev_yaw_rate_max; - float q_cov_slip_rate_min; - float q_cov_slip_rate_max; - float q_max_slip_angle; - float p0_cov_vel; - float p0_cov_slip; - float r_cov_x; - float r_cov_y; - float r_cov_yaw; - float r_cov_vel; - float p0_cov_x; - float p0_cov_y; - float p0_cov_yaw; + double r_cov_x; + double r_cov_y; + double r_cov_yaw; + double r_cov_vel; } ekf_params_; - double max_vel_; - double max_slip_; - double lf_; - double lr_; - float z_; double velocity_deviation_threshold_; -private: + double z_; + struct BoundingBox { double length; @@ -70,6 +51,12 @@ class BigVehicleTracker : public Tracker BoundingBox bounding_box_; BoundingBox last_input_bounding_box_; Eigen::Vector2d tracking_offset_; + int last_nearest_corner_index_; + +private: + BicycleMotionModel motion_model_; + const char DIM = motion_model_.DIM; + using IDX = BicycleMotionModel::IDX; public: BigVehicleTracker( @@ -77,18 +64,28 @@ class BigVehicleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; - bool predict(const double dt, KalmanFilter & ekf) const; bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const override; - double getMeasurementYaw(const autoware_auto_perception_msgs::msg::DetectedObject & object); - void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform); virtual ~BigVehicleTracker() {} + +private: + void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform) + { + Eigen::MatrixXd X_t(DIM, 1); + motion_model_.getStateVector(X_t); + last_nearest_corner_index_ = utils::getNearestCornerOrSurface( + X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, + self_transform); + } }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 05fa0a5ef01ba..6ab7e63bce167 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -19,7 +19,8 @@ #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ -#include "tracker_base.hpp" +#include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" #include @@ -28,42 +29,19 @@ class NormalVehicleTracker : public Tracker private: autoware_auto_perception_msgs::msg::DetectedObject object_; rclcpp::Logger logger_; - int last_nearest_corner_index_; private: - KalmanFilter ekf_; - rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; - struct EkfParams { - char dim_x = 5; - float q_stddev_acc_long; - float q_stddev_acc_lat; - float q_stddev_yaw_rate_min; - float q_stddev_yaw_rate_max; - float q_cov_slip_rate_min; - float q_cov_slip_rate_max; - float q_max_slip_angle; - float p0_cov_vel; - float p0_cov_slip; - float r_cov_x; - float r_cov_y; - float r_cov_yaw; - float r_cov_vel; - float p0_cov_x; - float p0_cov_y; - float p0_cov_yaw; + double r_cov_x; + double r_cov_y; + double r_cov_yaw; + double r_cov_vel; } ekf_params_; - - double max_vel_; - double max_slip_; - double lf_; - double lr_; - float z_; double velocity_deviation_threshold_; -private: + double z_; + struct BoundingBox { double length; @@ -73,6 +51,12 @@ class NormalVehicleTracker : public Tracker BoundingBox bounding_box_; BoundingBox last_input_bounding_box_; Eigen::Vector2d tracking_offset_; + int last_nearest_corner_index_; + +private: + BicycleMotionModel motion_model_; + const char DIM = motion_model_.DIM; + using IDX = BicycleMotionModel::IDX; public: NormalVehicleTracker( @@ -80,18 +64,28 @@ class NormalVehicleTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; - bool predict(const double dt, KalmanFilter & ekf) const; bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const override; - void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform); - double getMeasurementYaw(const autoware_auto_perception_msgs::msg::DetectedObject & object); virtual ~NormalVehicleTracker() {} + +private: + void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform) + { + Eigen::MatrixXd X_t(DIM, 1); + motion_model_.getStateVector(X_t); + last_nearest_corner_index_ = utils::getNearestCornerOrSurface( + X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, + self_transform); + } }; #endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index 0219d3b227044..a56250390e34f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -20,9 +20,12 @@ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ #include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" #include +// cspell: ignore CTRV + class PedestrianTracker : public Tracker { private: @@ -30,38 +33,15 @@ class PedestrianTracker : public Tracker rclcpp::Logger logger_; private: - KalmanFilter ekf_; - rclcpp::Time last_update_time_; - enum IDX { - X = 0, - Y = 1, - YAW = 2, - VX = 3, - WZ = 4, - }; struct EkfParams { - char dim_x = 5; - float q_cov_x; - float q_cov_y; - float q_cov_yaw; - float q_cov_wz; - float q_cov_vx; - float p0_cov_vx; - float p0_cov_wz; - float r_cov_x; - float r_cov_y; - float r_cov_yaw; - float p0_cov_x; - float p0_cov_y; - float p0_cov_yaw; + double r_cov_x; + double r_cov_y; + double r_cov_yaw; } ekf_params_; - double max_vx_; - double max_wz_; - float z_; + double z_; -private: struct BoundingBox { double length; @@ -76,16 +56,23 @@ class PedestrianTracker : public Tracker BoundingBox bounding_box_; Cylinder cylinder_; +private: + CTRVMotionModel motion_model_; + const char DIM = motion_model_.DIM; + using IDX = CTRVMotionModel::IDX; + public: PedestrianTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; - bool predict(const double dt, KalmanFilter & ekf) const; bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp index e7bf8030cddf8..ab6a747288d4a 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp @@ -42,6 +42,8 @@ class Tracker { classification_ = classification; } + void updateClassification( + const std::vector & classification); private: unique_identifier_msgs::msg::UUID uuid_; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp index 18e6c0606a535..73bf7849e13d8 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -13,13 +13,14 @@ // limitations under the License. // // -// v1.0 Yukihiro Saito +// Author: v1.0 Yukihiro Saito // #ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ #define MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ -#include "tracker_base.hpp" +#include "multi_object_tracker/tracker/model/tracker_base.hpp" +#include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" #include @@ -30,30 +31,20 @@ class UnknownTracker : public Tracker rclcpp::Logger logger_; private: - KalmanFilter ekf_; - rclcpp::Time last_update_time_; - enum IDX { - X = 0, - Y = 1, - VX = 2, - VY = 3, - }; struct EkfParams { - char dim_x = 4; - float q_cov_x; - float q_cov_y; - float q_cov_vx; - float q_cov_vy; - float p0_cov_vx; - float p0_cov_vy; - float r_cov_x; - float r_cov_y; - float p0_cov_x; - float p0_cov_y; + double r_cov_x; + double r_cov_y; + double r_cov_vx; + double r_cov_vy; } ekf_params_; - float max_vx_, max_vy_; - float z_; + + double z_; + +private: + CVMotionModel motion_model_; + const char DIM = motion_model_.DIM; + using IDX = CVMotionModel::IDX; public: UnknownTracker( @@ -61,10 +52,12 @@ class UnknownTracker : public Tracker const geometry_msgs::msg::Transform & self_transform); bool predict(const rclcpp::Time & time) override; - bool predict(const double dt, KalmanFilter & ekf) const; bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; + autoware_auto_perception_msgs::msg::DetectedObject getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp new file mode 100644 index 0000000000000..5127d0448835c --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp @@ -0,0 +1,108 @@ +// 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. +// +// +// Author: v1.0 Taekjin Lee +// + +#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ + +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" + +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +class BicycleMotionModel : public MotionModel +{ +private: + // attributes + rclcpp::Logger logger_; + + // extended state + double lf_; + double lr_; + + // motion parameters + struct MotionParams + { + double q_stddev_acc_long; + double q_stddev_acc_lat; + double q_stddev_yaw_rate_min; + double q_stddev_yaw_rate_max; + double q_cov_slip_rate_min; + double q_cov_slip_rate_max; + double q_max_slip_angle; + double lf_ratio; + double lr_ratio; + double lf_min; + double lr_min; + double max_vel; + double max_slip; + } motion_params_; + +public: + BicycleMotionModel(); + + enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; + const char DIM = 5; + + bool initialize( + const rclcpp::Time & time, const double & x, const double & y, const double & yaw, + const std::array & pose_cov, const double & vel, const double & vel_cov, + const double & slip, const double & slip_cov, const double & length); + + void setDefaultParams(); + + void setMotionParams( + const double & q_stddev_acc_long, const double & q_stddev_acc_lat, + const double & q_stddev_yaw_rate_min, const double & q_stddev_yaw_rate_max, + const double & q_stddev_slip_rate_min, const double & q_stddev_slip_rate_max, + const double & q_max_slip_angle, const double & lf_ratio, const double & lf_min, + const double & lr_ratio, const double & lr_min); + + void setMotionLimits(const double & max_vel, const double & max_slip); + + bool updateStatePose(const double & x, const double & y, const std::array & pose_cov); + + bool updateStatePoseHead( + const double & x, const double & y, const double & yaw, + const std::array & pose_cov); + + bool updateStatePoseHeadVel( + const double & x, const double & y, const double & yaw, const std::array & pose_cov, + const double & vel, const std::array & twist_cov); + + bool adjustPosition(const double & x, const double & y); + + bool limitStates(); + + bool updateExtendedState(const double & length); + + bool predictStateStep(const double dt, KalmanFilter & ekf) const override; + + bool getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; +}; + +#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__BICYCLE_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp new file mode 100644 index 0000000000000..6b071eddec7a9 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp @@ -0,0 +1,95 @@ +// 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. +// +// +// Author: v1.0 Taekjin Lee +// + +#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ + +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" + +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +// cspell: ignore CTRV + +class CTRVMotionModel : public MotionModel +{ +private: + // attributes + rclcpp::Logger logger_; + + // motion parameters + struct MotionParams + { + double q_cov_x; + double q_cov_y; + double q_cov_yaw; + double q_cov_vel; + double q_cov_wz; + double max_vel; + double max_wz; + } motion_params_; + +public: + CTRVMotionModel(); + + enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, WZ = 4 }; + const char DIM = 5; + + bool initialize( + const rclcpp::Time & time, const double & x, const double & y, const double & yaw, + const std::array & pose_cov, const double & vel, const double & vel_cov, + const double & wz, const double & wz_cov); + + void setDefaultParams(); + + void setMotionParams( + const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_yaw, + const double & q_stddev_vx, const double & q_stddev_wz); + + void setMotionLimits(const double & max_vel, const double & max_wz); + + bool updateStatePose(const double & x, const double & y, const std::array & pose_cov); + + bool updateStatePoseHead( + const double & x, const double & y, const double & yaw, + const std::array & pose_cov); + + bool updateStatePoseHeadVel( + const double & x, const double & y, const double & yaw, const std::array & pose_cov, + const double & vel, const std::array & twist_cov); + + bool adjustPosition(const double & x, const double & y); + + bool limitStates(); + + bool predictStateStep(const double dt, KalmanFilter & ekf) const override; + + bool getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; +}; + +#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CTRV_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp new file mode 100644 index 0000000000000..59032706b00d6 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp @@ -0,0 +1,88 @@ +// 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. +// +// +// Author: v1.0 Taekjin Lee +// + +#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ + +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" + +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +class CVMotionModel : public MotionModel +{ +private: + // attributes + rclcpp::Logger logger_; + + // motion parameters + struct MotionParams + { + double q_cov_x; + double q_cov_y; + double q_cov_vx; + double q_cov_vy; + double max_vx; + double max_vy; + } motion_params_; + +public: + CVMotionModel(); + + enum IDX { X = 0, Y = 1, VX = 2, VY = 3 }; + const char DIM = 4; + + bool initialize( + const rclcpp::Time & time, const double & x, const double & y, + const std::array & pose_cov, const double & vx, const double & vy, + const std::array & twist_cov); + + void setDefaultParams(); + + void setMotionParams( + const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_vx, + const double & q_stddev_vy); + + void setMotionLimits(const double & max_vx, const double & max_vy); + + bool updateStatePose(const double & x, const double & y, const std::array & pose_cov); + + bool updateStatePoseVel( + const double & x, const double & y, const std::array & pose_cov, const double & vx, + const double & vy, const std::array & twist_cov); + + bool adjustPosition(const double & x, const double & y); + + bool limitStates(); + + bool predictStateStep(const double dt, KalmanFilter & ekf) const override; + + bool getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const override; +}; + +#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__CV_MOTION_MODEL_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp new file mode 100644 index 0000000000000..1aca602ed66a3 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/motion_model/motion_model_base.hpp @@ -0,0 +1,67 @@ +// 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. +// +// +// Author: v1.0 Taekjin Lee +// + +#ifndef MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ + +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +class MotionModel +{ +private: + bool is_initialized_{false}; + double dt_max_{0.11}; // [s] maximum time interval for prediction + +protected: + rclcpp::Time last_update_time_; + KalmanFilter ekf_; + +public: + MotionModel(); + virtual ~MotionModel() = default; + + bool checkInitialized() const { return is_initialized_; } + double getDeltaTime(const rclcpp::Time & time) const + { + return (time - last_update_time_).seconds(); + } + void setMaxDeltaTime(const double dt_max) { dt_max_ = dt_max; } + double getStateElement(unsigned int idx) const { return ekf_.getXelement(idx); } + void getStateVector(Eigen::MatrixXd & X) const { ekf_.getX(X); } + + bool initialize(const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P); + + bool predictState(const rclcpp::Time & time); + bool getPredictedState(const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const; + + virtual bool predictStateStep(const double dt, KalmanFilter & ekf) const = 0; + virtual bool getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const = 0; +}; + +#endif // MULTI_OBJECT_TRACKER__TRACKER__MOTION_MODEL__MOTION_MODEL_BASE_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index d2b6ee5de475e..b19b0ba7ee10c 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -343,6 +343,32 @@ inline void convertConvexHullToBoundingBox( output_object.shape.dimensions.z = height; } +inline bool getMeasurementYaw( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const double & predicted_yaw, + double & measurement_yaw) +{ + measurement_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + + // check orientation sign is known or not, and fix the limiting delta yaw + double limiting_delta_yaw = M_PI_2; + if ( + object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { + limiting_delta_yaw = M_PI; + } + // limiting delta yaw, even the availability is unknown + while (std::fabs(predicted_yaw - measurement_yaw) > limiting_delta_yaw) { + if (measurement_yaw < predicted_yaw) { + measurement_yaw += 2 * limiting_delta_yaw; + } else { + measurement_yaw -= 2 * limiting_delta_yaw; + } + } + // return false if the orientation is unknown + return object.kinematics.orientation_availability != + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; +} + } // namespace utils #endif // MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index 1d4d50c7eab4c..fa609ed9ff9c7 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -148,7 +148,7 @@ void TrackerDebugger::publishProcessingTime() processing_time_publisher_->publish( "debug/processing_time_ms", processing_time_ms); processing_time_publisher_->publish( - "debug/elapsed_time_from_sensor_input_ms", elapsed_time_from_sensor_input_ * 1e3); + "debug/pipeline_latency_ms", elapsed_time_from_sensor_input_ * 1e3); } } @@ -236,6 +236,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) data_association_ = std::make_unique( can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix, min_iou_matrix); + published_time_publisher_ = std::make_unique(this); } void MultiObjectTracker::onMeasurement( @@ -256,7 +257,7 @@ void MultiObjectTracker::onMeasurement( return; } /* tracker prediction */ - rclcpp::Time measurement_time = input_objects_msg->header.stamp; + const rclcpp::Time measurement_time = input_objects_msg->header.stamp; for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { (*itr)->predict(measurement_time); } @@ -330,7 +331,7 @@ std::shared_ptr MultiObjectTracker::createNewTracker( void MultiObjectTracker::onTimer() { - rclcpp::Time current_time = this->now(); + const rclcpp::Time current_time = this->now(); const auto self_transform = getTransformAnonymous(tf_buffer_, world_frame_id_, "base_link", current_time); if (!self_transform) { @@ -373,10 +374,10 @@ void MultiObjectTracker::sanitizeTracker( /* delete collision tracker */ for (auto itr1 = list_tracker.begin(); itr1 != list_tracker.end(); ++itr1) { autoware_auto_perception_msgs::msg::TrackedObject object1; - (*itr1)->getTrackedObject(time, object1); + if (!(*itr1)->getTrackedObject(time, object1)) continue; for (auto itr2 = std::next(itr1); itr2 != list_tracker.end(); ++itr2) { autoware_auto_perception_msgs::msg::TrackedObject object2; - (*itr2)->getTrackedObject(time, object2); + if (!(*itr2)->getTrackedObject(time, object2)) continue; const double distance = std::hypot( object1.kinematics.pose_with_covariance.pose.position.x - object2.kinematics.pose_with_covariance.pose.position.x, @@ -441,7 +442,7 @@ inline bool MultiObjectTracker::shouldTrackerPublish( return true; } -void MultiObjectTracker::publish(const rclcpp::Time & time) +void MultiObjectTracker::publish(const rclcpp::Time & time) const { const auto subscriber_count = tracked_objects_pub_->get_subscription_count() + tracked_objects_pub_->get_intra_process_subscription_count(); @@ -457,17 +458,18 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { if (!shouldTrackerPublish(*itr)) { // for debug purpose autoware_auto_perception_msgs::msg::TrackedObject object; - (*itr)->getTrackedObject(time, object); + if (!(*itr)->getTrackedObject(time, object)) continue; tentative_objects_msg.objects.push_back(object); continue; } autoware_auto_perception_msgs::msg::TrackedObject object; - (*itr)->getTrackedObject(time, object); + if (!(*itr)->getTrackedObject(time, object)) continue; output_msg.objects.push_back(object); } // Publish tracked_objects_pub_->publish(output_msg); + published_time_publisher_->publish_if_subscribed(tracked_objects_pub_, output_msg.header.stamp); // Debugger Publish if enabled debugger_->publishProcessingTime(); diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 8a3a5629ec277..12c340516c6c1 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -47,399 +47,232 @@ BicycleTracker::BicycleTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("BicycleTracker")), - last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; // Initialize parameters // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty - float r_stddev_x = 0.5; // in vehicle coordinate [m] - float r_stddev_y = 0.4; // in vehicle coordinate [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] + double r_stddev_x = 0.5; // in vehicle coordinate [m] + double r_stddev_y = 0.4; // in vehicle coordinate [m] + double r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // in map coordinate [rad] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); - // initial state covariance - float p0_stddev_x = 0.8; // [m] - float p0_stddev_y = 0.5; // [m] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] - float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] - ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); - ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); - ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); - ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - // process noise covariance - ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - ekf_params_.q_stddev_yaw_rate_min = - tier4_autoware_utils::deg2rad(5.0); // [rad/s] uncertain yaw change rate - ekf_params_.q_stddev_yaw_rate_max = - tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate - float q_stddev_slip_rate_min = - tier4_autoware_utils::deg2rad(1); // [rad/s] uncertain slip angle change rate - float q_stddev_slip_rate_max = - tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate - ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); - ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); - ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle - // limitations - max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] - - // initialize state vector X - Eigen::MatrixXd X(ekf_params_.dim_x, 1); - X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; - X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; - X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - X(IDX::SLIP) = 0.0; - if (object.kinematics.has_twist) { - X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; - } else { - X(IDX::VEL) = 0.0; - } - - // initialize state covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(X(IDX::YAW)); - const double sin_yaw = std::sin(X(IDX::YAW)); - const double sin_2yaw = std::sin(2.0f * X(IDX::YAW)); - // Rotate the covariance matrix according to the vehicle yaw - // because p0_cov_x and y are in the vehicle coordinate system. - P(IDX::X, IDX::X) = - ekf_params_.p0_cov_x * cos_yaw * cos_yaw + ekf_params_.p0_cov_y * sin_yaw * sin_yaw; - P(IDX::X, IDX::Y) = 0.5f * (ekf_params_.p0_cov_x - ekf_params_.p0_cov_y) * sin_2yaw; - P(IDX::Y, IDX::Y) = - ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; - P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); - P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } else { - P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - P(IDX::YAW, IDX::YAW) = - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - if (object.kinematics.has_twist_covariance) { - P(IDX::VEL, IDX::VEL) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - } else { - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - } - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } + // OBJECT SHAPE MODEL if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; } else { bounding_box_ = {1.0, 0.5, 1.7}; } - ekf_.init(X, P); - - // Set lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% front from the center, minimum of 0.3m - lr_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% rear from the center, minimum of 0.3m -} + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); -bool BicycleTracker::predict(const rclcpp::Time & time) -{ - const double dt = (time - last_update_time_).seconds(); - if (dt < 0.0) { - RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); - return false; - } - // if dt is too large, shorten dt and repeat prediction - const double dt_max = 0.11; - const uint32_t repeat = std::ceil(dt / dt_max); - const double dt_ = dt / repeat; - bool ret = false; - for (uint32_t i = 0; i < repeat; ++i) { - ret = predict(dt_, ekf_); - if (!ret) { - return false; - } - } - if (ret) { - last_update_time_ = time; + // Set motion model parameters + { + constexpr double q_stddev_acc_long = + 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + constexpr double q_stddev_yaw_rate_min = 5.0; // [deg/s] uncertain yaw change rate, minimum + constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum + constexpr double q_stddev_slip_rate_min = + 1.0; // [deg/s] uncertain slip angle change rate, minimum + constexpr double q_stddev_slip_rate_max = + 10.0; // [deg/s] uncertain slip angle change rate, maximum + constexpr double q_max_slip_angle = 30; // [deg] max slip angle + constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position + constexpr double lf_min = 0.3; // [m] minimum front wheel position + constexpr double lr_ratio = 0.3; // [-] ratio of rear wheel position + constexpr double lr_min = 0.3; // [m] minimum rear wheel position + motion_model_.setMotionParams( + q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, + q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, + lr_min); } - return ret; -} -bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const -{ - /* Motion model: static bicycle model (constant slip angle, constant velocity) - * - * w_k = vel_k * sin(slip_k) / l_r - * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt - * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt - * yaw_{k+1} = yaw_k + w_k * dt - * vel_{k+1} = vel_k - * slip_{k+1} = slip_k - * - */ - - /* Jacobian Matrix - * - * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, - * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, - * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] - * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, - * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, - * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] - * - */ - - // Current state vector X t - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf.getX(X_t); - const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); - const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); - const double vel = X_t(IDX::VEL); - const double cos_slip = std::cos(X_t(IDX::SLIP)); - const double sin_slip = std::sin(X_t(IDX::SLIP)); - - double w = vel * sin_slip / lr_; - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - const double w_dtdt = w * dt * dt; - const double vv_dtdt__lr = vel * vel * dt * dt / lr_; - - // Predict state vector X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = - X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt - X_next_t(IDX::Y) = - X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt - X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt - X_next_t(IDX::VEL) = X_t(IDX::VEL); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) - - // State transition matrix A - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; - A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; - A(IDX::X, IDX::SLIP) = - -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; - A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; - A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; - A(IDX::Y, IDX::SLIP) = - vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; - A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; - - // Process noise covariance Q - float q_stddev_yaw_rate{0.0}; - if (vel <= 0.01) { - q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; - } else { - // uncertainty of the yaw rate is limited by - // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v - // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r - q_stddev_yaw_rate = std::min( - ekf_params_.q_stddev_acc_lat / vel, - vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] - q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); - q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); - } - float q_cov_slip_rate{0.0f}; - if (vel <= 0.01) { - q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; - } else { - // The slip angle rate uncertainty is modeled as follows: - // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt - // where sin(slip) = w * l_r / v - // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) - // d(v)/dt and d(w)/t are considered to be uncorrelated - q_cov_slip_rate = - std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); - q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); - q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); - } - const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); - const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); - const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); - const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); - const float q_cov_slip = q_cov_slip_rate * dt * dt; - - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Rotate the covariance matrix according to the vehicle yaw - // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); - Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); - Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); - Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = q_cov_yaw; - Q(IDX::VEL, IDX::VEL) = q_cov_vel; - Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; - - // control-input model B and control-input u are not used - // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); - - if (!ekf.predict(X_next_t, A, Q)) { - RCLCPP_WARN(logger_, "Cannot predict"); + // Set motion limits + { + constexpr double max_vel = tier4_autoware_utils::kmph2mps(80); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle + motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } - return true; -} + // Set initial state + { + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + auto pose_cov = object.kinematics.pose_with_covariance.covariance; + double vel = 0.0; + double vel_cov; + const double & length = bounding_box_.length; + + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; + } -bool BicycleTracker::measureWithPose( - const autoware_auto_perception_msgs::msg::DetectedObject & object) -{ - // yaw measurement - double measurement_yaw = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - - // prediction - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); - - // validate if orientation is available - bool use_orientation_information = false; - if ( - object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN) { // has 180 degree - // uncertainty - // fix orientation - while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - measurement_yaw = measurement_yaw + M_PI; + if (!object.kinematics.has_position_covariance) { + // initial state covariance + constexpr double p0_stddev_x = 0.8; // in object coordinate [m] + constexpr double p0_stddev_y = 0.5; // in object coordinate [m] + constexpr double p0_stddev_yaw = + tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); + constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); + constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + const double sin_2yaw = std::sin(2.0 * yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[utils::MSG_COV_IDX::Y_Y] = + p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_yaw; } - while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - measurement_yaw = measurement_yaw - M_PI; + + if (!object.kinematics.has_twist_covariance) { + constexpr double p0_stddev_vel = + tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + vel_cov = std::pow(p0_stddev_vel, 2.0); + } else { + vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } - use_orientation_information = true; - } else if ( - object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { // know full angle + const double slip = 0.0; + const double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] + const double slip_cov = std::pow(p0_stddev_slip, 2.0); - use_orientation_information = true; + // initialize motion model + motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); } +} - const int dim_y = - use_orientation_information ? 3 : 2; // pos x, pos y, (pos yaw) depending on Pose output +bool BicycleTracker::predict(const rclcpp::Time & time) +{ + return motion_model_.predictState(time); +} - // Set measurement matrix C and observation vector Y - Eigen::MatrixXd Y(dim_y, 1); - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Y(IDX::X, 0) = object.kinematics.pose_with_covariance.pose.position.x; - Y(IDX::Y, 0) = object.kinematics.pose_with_covariance.pose.position.y; - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y +autoware_auto_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & /*self_transform*/) +{ + autoware_auto_perception_msgs::msg::DetectedObject updating_object; - // Set noise covariance matrix R - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { - R(0, 0) = ekf_params_.r_cov_x; // x - x - R(0, 1) = 0.0; // x - y - R(1, 1) = ekf_params_.r_cov_y; // y - y - R(1, 0) = R(0, 1); // y - x + // OBJECT SHAPE MODEL + // convert to bounding box if input is convex shape + if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + utils::convertConvexHullToBoundingBox(object, updating_object); } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + updating_object = object; } - // if there are orientation available - if (dim_y == 3) { - // fill yaw observation and measurement matrix - Y(IDX::YAW, 0) = measurement_yaw; - C(2, IDX::YAW) = 1.0; - - // fill yaw covariance - if (!object.kinematics.has_position_covariance) { - R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw - } else { - R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; - R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; - R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; - R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - } + // UNCERTAINTY MODEL + if (!object.kinematics.has_position_covariance) { + // fill covariance matrix + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + pose_cov[utils::MSG_COV_IDX::X_X] = ekf_params_.r_cov_x; // x - x + pose_cov[utils::MSG_COV_IDX::X_Y] = 0; // x - y + pose_cov[utils::MSG_COV_IDX::Y_X] = 0; // y - x + pose_cov[utils::MSG_COV_IDX::Y_Y] = ekf_params_.r_cov_y; // y - y + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = ekf_params_.r_cov_yaw; // yaw - yaw } - // ekf update - if (!ekf_.update(Y, C, R)) { - RCLCPP_WARN(logger_, "Cannot update"); - } + return updating_object; +} - // normalize yaw and limit vel, slip +bool BicycleTracker::measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + // get measurement yaw angle to update + const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); + double measurement_yaw = 0.0; + bool is_yaw_available = utils::getMeasurementYaw(object, tracked_yaw, measurement_yaw); + + // update + bool is_updated = false; { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { - X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; - } - if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { - X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = measurement_yaw; + + if (is_yaw_available) { + is_updated = motion_model_.updateStatePoseHead( + x, y, yaw, object.kinematics.pose_with_covariance.covariance); + } else { + is_updated = + motion_model_.updateStatePose(x, y, object.kinematics.pose_with_covariance.covariance); } - ekf_.init(X_t, P_t); + motion_model_.limitStates(); } // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + constexpr double gain = 0.1; + z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; - return true; + return is_updated; } bool BicycleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - // if the input shape is convex type, convert it to bbox type - autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - utils::convertConvexHullToBoundingBox(object, bbox_object); - } else { - bbox_object = object; + if (!object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + // do not update shape if the input is not a bounding box + return true; } - constexpr float gain = 0.9; - bounding_box_.length = - gain * bounding_box_.length + (1.0 - gain) * bbox_object.shape.dimensions.x; - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * bbox_object.shape.dimensions.y; - bounding_box_.height = - gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; - last_input_bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + constexpr double gain = 0.1; + constexpr double gain_inv = 1.0 - gain; - // update lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% front from the center, minimum of 0.3m - lr_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% rear from the center, minimum of 0.3m + // update object size + bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; + bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; + bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); + + // update motion model + motion_model_.updateExtendedState(bounding_box_.length); return true; } bool BicycleTracker::measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & /*self_transform*/) + const geometry_msgs::msg::Transform & self_transform) { - const auto & current_classification = getClassification(); + // keep the latest input object object_ = object; + + // update classification + const auto & current_classification = getClassification(); if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + // check time gap + const double dt = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < dt) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + logger_, + "BicycleTracker::measure There is a large gap between predicted time and measurement time. " + "(%f)", + dt); } - measureWithPose(object); - measureWithShape(object); + // update object + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = + getUpdatingObject(object, self_transform); + measureWithPose(updating_object); + measureWithShape(updating_object); return true; } @@ -451,92 +284,19 @@ bool BicycleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict state - KalmanFilter tmp_ekf_for_no_update = ekf_; - const double dt = (time - last_update_time_).seconds(); - if (0.001 /*1msec*/ < dt) { - predict(dt, tmp_ekf_for_no_update); - } - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - Eigen::MatrixXd P(ekf_params_.dim_x, ekf_params_.dim_x); // predicted state - tmp_ekf_for_no_update.getX(X_t); - tmp_ekf_for_no_update.getP(P); - - /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + // predict from motion model + if (!motion_model_.getPredictedState( + time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, + twist_with_cov.covariance)) { + RCLCPP_WARN(logger_, "BicycleTracker::getTrackedObject: Failed to get predicted state."); + return false; + } + // position - pose_with_cov.pose.position.x = X_t(IDX::X); - pose_with_cov.pose.position.y = X_t(IDX::Y); pose_with_cov.pose.position.z = z_; - // quaternion - { - double roll, pitch, yaw; - tf2::Quaternion original_quaternion; - tf2::fromMsg(object_.kinematics.pose_with_covariance.pose.orientation, original_quaternion); - tf2::Matrix3x3(original_quaternion).getRPY(roll, pitch, yaw); - tf2::Quaternion filtered_quaternion; - filtered_quaternion.setRPY(roll, pitch, X_t(IDX::YAW)); - pose_with_cov.pose.orientation.x = filtered_quaternion.x(); - pose_with_cov.pose.orientation.y = filtered_quaternion.y(); - pose_with_cov.pose.orientation.z = filtered_quaternion.z(); - pose_with_cov.pose.orientation.w = filtered_quaternion.w(); - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; - } - // position covariance - constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double p_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = r_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); - - // twist - twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); - twist_with_cov.twist.angular.z = - X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r - /* twist covariance - * convert covariance from velocity, slip angle to vx, vy, and yaw angle - * - * vx = vel * cos(slip) - * vy = vel * sin(slip) - * wz = vel * sin(slip) / l_r = vy / l_r - * - */ - - constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - - Eigen::MatrixXd cov_jacob(3, 2); - cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; - Eigen::MatrixXd cov_twist(2, 2); - cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), - P(IDX::SLIP, IDX::SLIP); - Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); - - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; // set shape object.shape.dimensions.x = bounding_box_.length; diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 80c0df7e5ffb1..3c73b9f19cfbb 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -47,7 +47,6 @@ BigVehicleTracker::BigVehicleTracker( const geometry_msgs::msg::Transform & self_transform) : Tracker(time, object.classification), logger_(rclcpp::get_logger("BigVehicleTracker")), - last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { @@ -57,94 +56,24 @@ BigVehicleTracker::BigVehicleTracker( // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty float r_stddev_x = 0.5; // in vehicle coordinate [m] float r_stddev_y = 0.4; // in vehicle coordinate [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad] - float r_stddev_vel = 1.0; // [m/s] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // in map coordinate [rad] + float r_stddev_vel = 1.0; // in object coordinate [m/s] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0); - // initial state covariance - float p0_stddev_x = 1.5; // [m] - float p0_stddev_y = 0.5; // [m] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] - float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] - ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); - ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); - ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); - ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - // process noise covariance - ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - ekf_params_.q_stddev_yaw_rate_min = - tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate - ekf_params_.q_stddev_yaw_rate_max = - tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate - float q_stddev_slip_rate_min = - tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate - float q_stddev_slip_rate_max = - tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate - ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); - ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); - ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle - // limitations - max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] - velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - // initialize state vector X - Eigen::MatrixXd X(ekf_params_.dim_x, 1); - X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; - X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; - X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - X(IDX::SLIP) = 0.0; - if (object.kinematics.has_twist) { - X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; - } else { - X(IDX::VEL) = 0.0; - } - - // initialize state covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(X(IDX::YAW)); - const double sin_yaw = std::sin(X(IDX::YAW)); - const double sin_2yaw = std::sin(2.0f * X(IDX::YAW)); - // Rotate the covariance matrix according to the vehicle yaw - // because p0_cov_x and y are in the vehicle coordinate system. - P(IDX::X, IDX::X) = - ekf_params_.p0_cov_x * cos_yaw * cos_yaw + ekf_params_.p0_cov_y * sin_yaw * sin_yaw; - P(IDX::X, IDX::Y) = 0.5f * (ekf_params_.p0_cov_x - ekf_params_.p0_cov_y) * sin_2yaw; - P(IDX::Y, IDX::Y) = - ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; - P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); - P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } else { - P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - P(IDX::YAW, IDX::YAW) = - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - if (object.kinematics.has_twist_covariance) { - P(IDX::VEL, IDX::VEL) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - } else { - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - } - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } + // velocity deviation threshold + // if the predicted velocity is close to the observed velocity, + // the observed velocity is used as the measurement. + velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] + // OBJECT SHAPE MODEL if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } else { - // past default value - // bounding_box_ = {7.0, 2.0, 2.0}; autoware_auto_perception_msgs::msg::DetectedObject bbox_object; utils::convertConvexHullToBoundingBox(object, bbox_object); bounding_box_ = { @@ -152,203 +81,114 @@ BigVehicleTracker::BigVehicleTracker( bbox_object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } - ekf_.init(X, P); - - /* calc nearest corner index*/ - setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); - // Set lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m - lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m -} + // Set motion model parameters + { + constexpr double q_stddev_acc_long = + 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate, minimum + constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum + constexpr double q_stddev_slip_rate_min = + 0.3; // [deg/s] uncertain slip angle change rate, minimum + constexpr double q_stddev_slip_rate_max = + 10.0; // [deg/s] uncertain slip angle change rate, maximum + constexpr double q_max_slip_angle = 30; // [deg] max slip angle + constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position + constexpr double lf_min = 1.5; // [m] minimum front wheel position + constexpr double lr_ratio = 0.25; // [-] ratio of rear wheel position + constexpr double lr_min = 1.5; // [m] minimum rear wheel position + motion_model_.setMotionParams( + q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, + q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, + lr_min); + } -bool BigVehicleTracker::predict(const rclcpp::Time & time) -{ - const double dt = (time - last_update_time_).seconds(); - if (dt < 0.0) { - RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); - return false; + // Set motion limits + { + constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle + motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } - // if dt is too large, shorten dt and repeat prediction - const double dt_max = 0.11; - const uint32_t repeat = std::ceil(dt / dt_max); - const double dt_ = dt / repeat; - bool ret = false; - for (uint32_t i = 0; i < repeat; ++i) { - ret = predict(dt_, ekf_); - if (!ret) { - return false; + + // Set initial state + { + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + auto pose_cov = object.kinematics.pose_with_covariance.covariance; + double vel = 0.0; + double vel_cov; + const double & length = bounding_box_.length; + + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; } - } - if (ret) { - last_update_time_ = time; - } - return ret; -} -bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const -{ - /* Motion model: static bicycle model (constant slip angle, constant velocity) - * - * w_k = vel_k * sin(slip_k) / l_r - * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt - * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt - * yaw_{k+1} = yaw_k + w_k * dt - * vel_{k+1} = vel_k - * slip_{k+1} = slip_k - * - */ - - /* Jacobian Matrix - * - * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, - * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, - * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] - * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, - * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, - * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] - * - */ - - // Current state vector X t - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf.getX(X_t); - const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); - const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); - const double vel = X_t(IDX::VEL); - const double cos_slip = std::cos(X_t(IDX::SLIP)); - const double sin_slip = std::sin(X_t(IDX::SLIP)); - - double w = vel * sin_slip / lr_; - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - const double w_dtdt = w * dt * dt; - const double vv_dtdt__lr = vel * vel * dt * dt / lr_; - - // Predict state vector X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = - X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt - X_next_t(IDX::Y) = - X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt - X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt - X_next_t(IDX::VEL) = X_t(IDX::VEL); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) - - // State transition matrix A - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; - A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; - A(IDX::X, IDX::SLIP) = - -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; - A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; - A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; - A(IDX::Y, IDX::SLIP) = - vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; - A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; - - // Process noise covariance Q - float q_stddev_yaw_rate{0.0}; - if (vel <= 0.01) { - q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; - } else { - // uncertainty of the yaw rate is limited by - // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v - // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r - q_stddev_yaw_rate = std::min( - ekf_params_.q_stddev_acc_lat / vel, - vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] - q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); - q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); - } - float q_cov_slip_rate{0.0f}; - if (vel <= 0.01) { - q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; - } else { - // The slip angle rate uncertainty is modeled as follows: - // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt - // where sin(slip) = w * l_r / v - // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) - // d(v)/dt and d(w)/t are considered to be uncorrelated - q_cov_slip_rate = - std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); - q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); - q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); - } - const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); - const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); - const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); - const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); - const float q_cov_slip = q_cov_slip_rate * dt * dt; - - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Rotate the covariance matrix according to the vehicle yaw - // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); - Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); - Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); - Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = q_cov_yaw; - Q(IDX::VEL, IDX::VEL) = q_cov_vel; - Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; - - // control-input model B and control-input u are not used - // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); - - if (!ekf.predict(X_next_t, A, Q)) { - RCLCPP_WARN(logger_, "Cannot predict"); + if (!object.kinematics.has_position_covariance) { + // initial state covariance + constexpr double p0_stddev_x = 1.5; // in object coordinate [m] + constexpr double p0_stddev_y = 0.5; // in object coordinate [m] + constexpr double p0_stddev_yaw = + tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); + constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); + constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + const double sin_2yaw = std::sin(2.0 * yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[utils::MSG_COV_IDX::Y_Y] = + p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_yaw; + } + + if (!object.kinematics.has_twist_covariance) { + constexpr double p0_stddev_vel = + tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + vel_cov = std::pow(p0_stddev_vel, 2.0); + } else { + vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + } + + const double slip = 0.0; + const double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] + const double slip_cov = std::pow(p0_stddev_slip, 2.0); + + // initialize motion model + motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); } - return true; + /* calc nearest corner index*/ + setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step } -bool BigVehicleTracker::measureWithPose( - const autoware_auto_perception_msgs::msg::DetectedObject & object) +bool BigVehicleTracker::predict(const rclcpp::Time & time) { - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; - - float r_cov_x; - float r_cov_y; - const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - - if (utils::isLargeVehicleLabel(label)) { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; - } else if (label == Label::CAR) { - constexpr float r_stddev_x = 2.0; // [m] - constexpr float r_stddev_y = 2.0; // [m] - r_cov_x = std::pow(r_stddev_x, 2.0); - r_cov_y = std::pow(r_stddev_y, 2.0); - } else { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; - } - - // Decide dimension of measurement vector - bool enable_velocity_measurement = false; - if (object.kinematics.has_twist) { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); - const double predicted_vel = X_t(IDX::VEL); - const double observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; + return motion_model_.predictState(time); +} - if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { - // Velocity deviation is small - enable_velocity_measurement = true; - } - } +autoware_auto_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) +{ + autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; - // pos x, pos y, yaw, vel depending on pose measurement - const int dim_y = enable_velocity_measurement ? 4 : 3; - double measurement_yaw = getMeasurementYaw(object); // get sign-solved yaw angle - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); + // current (predicted) state + const double tracked_x = motion_model_.getStateElement(IDX::X); + const double tracked_y = motion_model_.getStateElement(IDX::Y); + const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); - // convert to boundingbox if input is convex shape + // OBJECT SHAPE MODEL + // convert to bounding box if input is convex shape autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { utils::convertConvexHullToBoundingBox(object, bbox_object); @@ -356,108 +196,134 @@ bool BigVehicleTracker::measureWithPose( bbox_object = object; } - /* get offset measurement*/ - autoware_auto_perception_msgs::msg::DetectedObject offset_object; + // get offset measurement + int nearest_corner_index = utils::getNearestCornerOrSurface( + tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); utils::calcAnchorPointOffset( - last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, - bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); - - // Set measurement matrix C and observation vector Y - Eigen::MatrixXd Y(dim_y, 1); - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x; - Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y; - Y(IDX::YAW, 0) = measurement_yaw; - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - C(2, IDX::YAW) = 1.0; // for yaw - - // Set noise covariance matrix R - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(measurement_yaw); - const double sin_yaw = std::sin(measurement_yaw); - const double sin_2yaw = std::sin(2.0f * measurement_yaw); - R(0, 0) = r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - R(0, 1) = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - R(1, 1) = r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - R(1, 0) = R(0, 1); // y - x - R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw - } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; - R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; - R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - } - - // Update the velocity when necessary - if (dim_y == 4) { - Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; - C(3, IDX::VEL) = 1.0; // for vel + last_input_bounding_box_.width, last_input_bounding_box_.length, nearest_corner_index, + bbox_object, tracked_yaw, updating_object, tracking_offset_); - if (!object.kinematics.has_twist_covariance) { - R(3, 3) = ekf_params_.r_cov_vel; // vel -vel + // UNCERTAINTY MODEL + if (!object.kinematics.has_position_covariance) { + // measurement noise covariance + float r_cov_x; + float r_cov_y; + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); + if (utils::isLargeVehicleLabel(label)) { + r_cov_x = ekf_params_.r_cov_x; + r_cov_y = ekf_params_.r_cov_y; + } else if (label == Label::CAR) { + // if label is changed, enlarge the measurement noise covariance + constexpr float r_stddev_x = 2.0; // [m] + constexpr float r_stddev_y = 2.0; // [m] + r_cov_x = std::pow(r_stddev_x, 2.0); + r_cov_y = std::pow(r_stddev_y, 2.0); } else { - R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + r_cov_x = ekf_params_.r_cov_x; + r_cov_y = ekf_params_.r_cov_y; + } + + // yaw angle fix + double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + bool is_yaw_available = + object.kinematics.orientation_availability != + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + + // fill covariance matrix + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + const double cos_yaw = std::cos(pose_yaw); + const double sin_yaw = std::sin(pose_yaw); + const double sin_2yaw = std::sin(2.0f * pose_yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[utils::MSG_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; // y - x + pose_cov[utils::MSG_COV_IDX::X_YAW] = 0.0; // x - yaw + pose_cov[utils::MSG_COV_IDX::Y_YAW] = 0.0; // y - yaw + pose_cov[utils::MSG_COV_IDX::YAW_X] = 0.0; // yaw - x + pose_cov[utils::MSG_COV_IDX::YAW_Y] = 0.0; // yaw - y + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = ekf_params_.r_cov_yaw; // yaw - yaw + if (!is_yaw_available) { + pose_cov[utils::MSG_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value } + auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; + twist_cov[utils::MSG_COV_IDX::X_X] = ekf_params_.r_cov_vel; // vel - vel } - // ekf update - if (!ekf_.update(Y, C, R)) { - RCLCPP_WARN(logger_, "Cannot update"); + return updating_object; +} + +bool BigVehicleTracker::measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + // current (predicted) state + const double tracked_vel = motion_model_.getStateElement(IDX::VEL); + + // velocity capability is checked only when the object has velocity measurement + // and the predicted velocity is close to the observed velocity + bool is_velocity_available = false; + if (object.kinematics.has_twist) { + const double & observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; + if (std::fabs(tracked_vel - observed_vel) < velocity_deviation_threshold_) { + // Velocity deviation is small + is_velocity_available = true; + } } - // normalize yaw and limit vel, slip + // update + bool is_updated = false; { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { - X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; - } - if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { - X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double vel = object.kinematics.twist_with_covariance.twist.linear.x; + + if (is_velocity_available) { + is_updated = motion_model_.updateStatePoseHeadVel( + x, y, yaw, object.kinematics.pose_with_covariance.covariance, vel, + object.kinematics.twist_with_covariance.covariance); + } else { + is_updated = motion_model_.updateStatePoseHead( + x, y, yaw, object.kinematics.pose_with_covariance.covariance); } - ekf_.init(X_t, P_t); + motion_model_.limitStates(); } // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + constexpr double gain = 0.1; + z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; - return true; + return is_updated; } bool BigVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - // if the input shape is convex type, convert it to bbox type - autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - utils::convertConvexHullToBoundingBox(object, bbox_object); - } else { - bbox_object = object; - } + constexpr double gain = 0.1; + constexpr double gain_inv = 1.0 - gain; - constexpr float gain = 0.9; - bounding_box_.length = - gain * bounding_box_.length + (1.0 - gain) * bbox_object.shape.dimensions.x; - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * bbox_object.shape.dimensions.y; - bounding_box_.height = - gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; + // update object size + bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; + bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; + bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; last_input_bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); - // update lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m - lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m + // update motion model + motion_model_.updateExtendedState(bounding_box_.length); + + // // update offset into object position + // motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y()); + // // update offset + // tracking_offset_.x() = gain_inv * tracking_offset_.x(); + // tracking_offset_.y() = gain_inv * tracking_offset_.y(); return true; } @@ -466,20 +332,30 @@ bool BigVehicleTracker::measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { - const auto & current_classification = getClassification(); + // keep the latest input object object_ = object; + + // update classification + const auto & current_classification = getClassification(); if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + // check time gap + const double dt = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < dt) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + logger_, + "BigVehicleTracker::measure There is a large gap between predicted time and measurement " + "time. (%f)", + dt); } - measureWithPose(object); - measureWithShape(object); + // update object + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = + getUpdatingObject(object, self_transform); + measureWithPose(updating_object); + measureWithShape(updating_object); /* calc nearest corner index*/ setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step @@ -494,100 +370,28 @@ bool BigVehicleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict state - KalmanFilter tmp_ekf_for_no_update = ekf_; - const double dt = (time - last_update_time_).seconds(); - if (0.001 /*1msec*/ < dt) { - predict(dt, tmp_ekf_for_no_update); - } - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - Eigen::MatrixXd P(ekf_params_.dim_x, ekf_params_.dim_x); // predicted state - tmp_ekf_for_no_update.getX(X_t); - tmp_ekf_for_no_update.getP(P); - - /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + // predict from motion model + if (!motion_model_.getPredictedState( + time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, + twist_with_cov.covariance)) { + RCLCPP_WARN(logger_, "BigVehicleTracker::getTrackedObject: Failed to get predicted state."); + return false; + } + // recover bounding box from tracking point const double dl = bounding_box_.length - last_input_bounding_box_.length; const double dw = bounding_box_.width - last_input_bounding_box_.width; const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); - X_t(IDX::X) = recovered_pose.x(); - X_t(IDX::Y) = recovered_pose.y(); + pose_with_cov.pose.position.x, pose_with_cov.pose.position.y, + motion_model_.getStateElement(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); + pose_with_cov.pose.position.x = recovered_pose.x(); + pose_with_cov.pose.position.y = recovered_pose.y(); // position - pose_with_cov.pose.position.x = X_t(IDX::X); - pose_with_cov.pose.position.y = X_t(IDX::Y); pose_with_cov.pose.position.z = z_; - // quaternion - { - double roll, pitch, yaw; - tf2::Quaternion original_quaternion; - tf2::fromMsg(object_.kinematics.pose_with_covariance.pose.orientation, original_quaternion); - tf2::Matrix3x3(original_quaternion).getRPY(roll, pitch, yaw); - tf2::Quaternion filtered_quaternion; - filtered_quaternion.setRPY(roll, pitch, X_t(IDX::YAW)); - pose_with_cov.pose.orientation.x = filtered_quaternion.x(); - pose_with_cov.pose.orientation.y = filtered_quaternion.y(); - pose_with_cov.pose.orientation.z = filtered_quaternion.z(); - pose_with_cov.pose.orientation.w = filtered_quaternion.w(); - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; - } - // position covariance - constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double p_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = r_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); - - // twist - twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); - twist_with_cov.twist.angular.z = - X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r - /* twist covariance - * convert covariance from velocity, slip angle to vx, vy, and yaw angle - * - * vx = vel * cos(slip) - * vy = vel * sin(slip) - * wz = vel * sin(slip) / l_r = vy / l_r - * - */ - - constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - - Eigen::MatrixXd cov_jacob(3, 2); - cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; - Eigen::MatrixXd cov_twist(2, 2); - cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), - P(IDX::SLIP, IDX::SLIP); - Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); - - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; // set shape object.shape.dimensions.x = bounding_box_.length; @@ -600,32 +404,3 @@ bool BigVehicleTracker::getTrackedObject( return true; } - -void BigVehicleTracker::setNearestCornerOrSurfaceIndex( - const geometry_msgs::msg::Transform & self_transform) -{ - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); - last_nearest_corner_index_ = utils::getNearestCornerOrSurface( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, - self_transform); -} - -double BigVehicleTracker::getMeasurementYaw( - const autoware_auto_perception_msgs::msg::DetectedObject & object) -{ - double measurement_yaw = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); - // Fixed measurement_yaw to be in the range of +-90 degrees of X_t(IDX::YAW) - while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - measurement_yaw = measurement_yaw + M_PI; - } - while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - measurement_yaw = measurement_yaw - M_PI; - } - } - return measurement_yaw; -} diff --git a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp index 51adca7e69b56..4f0fb4d7871f2 100644 --- a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp @@ -43,7 +43,7 @@ bool MultipleVehicleTracker::measure( big_vehicle_tracker_.measure(object, time, self_transform); normal_vehicle_tracker_.measure(object, time, self_transform); if (object_recognition_utils::getHighestProbLabel(object.classification) != Label::UNKNOWN) - setClassification(object.classification); + updateClassification(object.classification); return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 7714c381894f0..19fc5a2f71122 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -47,7 +47,6 @@ NormalVehicleTracker::NormalVehicleTracker( const geometry_msgs::msg::Transform & self_transform) : Tracker(time, object.classification), logger_(rclcpp::get_logger("NormalVehicleTracker")), - last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z), tracking_offset_(Eigen::Vector2d::Zero()) { @@ -63,88 +62,18 @@ NormalVehicleTracker::NormalVehicleTracker( ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0); - // initial state covariance - float p0_stddev_x = 1.0; // in object coordinate [m] - float p0_stddev_y = 0.3; // in object coordinate [m] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] - float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] - ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); - ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); - ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); - ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - // process noise covariance - ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - ekf_params_.q_stddev_yaw_rate_min = - tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate - ekf_params_.q_stddev_yaw_rate_max = - tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate - float q_stddev_slip_rate_min = - tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate - float q_stddev_slip_rate_max = - tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate - ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); - ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); - ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle - // limitations - max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] - velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - // initialize state vector X - Eigen::MatrixXd X(ekf_params_.dim_x, 1); - X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; - X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; - X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - X(IDX::SLIP) = 0.0; - if (object.kinematics.has_twist) { - X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; - } else { - X(IDX::VEL) = 0.0; - } - - // initialize state covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(X(IDX::YAW)); - const double sin_yaw = std::sin(X(IDX::YAW)); - const double sin_2yaw = std::sin(2.0f * X(IDX::YAW)); - // Rotate the covariance matrix according to the vehicle yaw - // because p0_cov_x and y are in the vehicle coordinate system. - P(IDX::X, IDX::X) = - ekf_params_.p0_cov_x * cos_yaw * cos_yaw + ekf_params_.p0_cov_y * sin_yaw * sin_yaw; - P(IDX::X, IDX::Y) = 0.5f * (ekf_params_.p0_cov_x - ekf_params_.p0_cov_y) * sin_2yaw; - P(IDX::Y, IDX::Y) = - ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; - P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); - P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } else { - P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - P(IDX::YAW, IDX::YAW) = - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - if (object.kinematics.has_twist_covariance) { - P(IDX::VEL, IDX::VEL) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - } else { - P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; - } - P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; - } + // velocity deviation threshold + // if the predicted velocity is close to the observed velocity, + // the observed velocity is used as the measurement. + velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] + // OBJECT SHAPE MODEL if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } else { - // past default value - // bounding_box_ = {4.0, 1.7, 2.0}; autoware_auto_perception_msgs::msg::DetectedObject bbox_object; utils::convertConvexHullToBoundingBox(object, bbox_object); bounding_box_ = { @@ -152,203 +81,114 @@ NormalVehicleTracker::NormalVehicleTracker( bbox_object.shape.dimensions.z}; last_input_bounding_box_ = bounding_box_; } - ekf_.init(X, P); - - /* calc nearest corner index*/ - setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); - // Set lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m - lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m -} + // Set motion model parameters + { + constexpr double q_stddev_acc_long = + 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate, minimum + constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate, maximum + constexpr double q_stddev_slip_rate_min = + 0.3; // [deg/s] uncertain slip angle change rate, minimum + constexpr double q_stddev_slip_rate_max = + 10.0; // [deg/s] uncertain slip angle change rate, maximum + constexpr double q_max_slip_angle = 30; // [deg] max slip angle + constexpr double lf_ratio = 0.3; // [-] ratio of front wheel position + constexpr double lf_min = 1.0; // [m] minimum front wheel position + constexpr double lr_ratio = 0.25; // [-] ratio of rear wheel position + constexpr double lr_min = 1.0; // [m] minimum rear wheel position + motion_model_.setMotionParams( + q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, + q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, + lr_min); + } -bool NormalVehicleTracker::predict(const rclcpp::Time & time) -{ - const double dt = (time - last_update_time_).seconds(); - if (dt < 0.0) { - RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); - return false; + // Set motion limits + { + constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30; // [deg] maximum slip angle + motion_model_.setMotionLimits(max_vel, max_slip); // maximum velocity and slip angle } - // if dt is too large, shorten dt and repeat prediction - const double dt_max = 0.11; - const uint32_t repeat = std::ceil(dt / dt_max); - const double dt_ = dt / repeat; - bool ret = false; - for (uint32_t i = 0; i < repeat; ++i) { - ret = predict(dt_, ekf_); - if (!ret) { - return false; + + // Set initial state + { + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + auto pose_cov = object.kinematics.pose_with_covariance.covariance; + double vel = 0.0; + double vel_cov; + const double & length = bounding_box_.length; + + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; } - } - if (ret) { - last_update_time_ = time; - } - return ret; -} -bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const -{ - /* Motion model: static bicycle model (constant slip angle, constant velocity) - * - * w_k = vel_k * sin(slip_k) / l_r - * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt - * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt - * yaw_{k+1} = yaw_k + w_k * dt - * vel_{k+1} = vel_k - * slip_{k+1} = slip_k - * - */ - - /* Jacobian Matrix - * - * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, - * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, - * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] - * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, - * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, - * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] - * - */ - - // Current state vector X t - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf.getX(X_t); - const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); - const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); - const double vel = X_t(IDX::VEL); - const double cos_slip = std::cos(X_t(IDX::SLIP)); - const double sin_slip = std::sin(X_t(IDX::SLIP)); - - double w = vel * sin_slip / lr_; - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - const double w_dtdt = w * dt * dt; - const double vv_dtdt__lr = vel * vel * dt * dt / lr_; - - // Predict state vector X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = - X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt - X_next_t(IDX::Y) = - X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt - X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt - X_next_t(IDX::VEL) = X_t(IDX::VEL); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) - - // State transition matrix A - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; - A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; - A(IDX::X, IDX::SLIP) = - -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; - A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; - A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; - A(IDX::Y, IDX::SLIP) = - vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; - A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; - - // Process noise covariance Q - float q_stddev_yaw_rate{0.0}; - if (vel <= 0.01) { - q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; - } else { - // uncertainty of the yaw rate is limited by - // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v - // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r - q_stddev_yaw_rate = std::min( - ekf_params_.q_stddev_acc_lat / vel, - vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] - q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); - q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); - } - float q_cov_slip_rate{0.0f}; - if (vel <= 0.01) { - q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; - } else { - // The slip angle rate uncertainty is modeled as follows: - // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt - // where sin(slip) = w * l_r / v - // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) - // d(v)/dt and d(w)/t are considered to be uncorrelated - q_cov_slip_rate = - std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); - q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); - q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); - } - const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); - const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); - const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); - const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); - const float q_cov_slip = q_cov_slip_rate * dt * dt; - - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Rotate the covariance matrix according to the vehicle yaw - // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); - Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); - Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); - Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = q_cov_yaw; - Q(IDX::VEL, IDX::VEL) = q_cov_vel; - Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; - - // control-input model B and control-input u are not used - // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); - - if (!ekf.predict(X_next_t, A, Q)) { - RCLCPP_WARN(logger_, "Cannot predict"); + if (!object.kinematics.has_position_covariance) { + // initial state covariance + constexpr double p0_stddev_x = 1.0; // in object coordinate [m] + constexpr double p0_stddev_y = 0.3; // in object coordinate [m] + constexpr double p0_stddev_yaw = + tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); + constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); + constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + const double sin_2yaw = std::sin(2.0 * yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[utils::MSG_COV_IDX::Y_Y] = + p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_yaw; + } + + if (!object.kinematics.has_twist_covariance) { + constexpr double p0_stddev_vel = + tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + vel_cov = std::pow(p0_stddev_vel, 2.0); + } else { + vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + } + + const double slip = 0.0; + const double p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] + const double slip_cov = std::pow(p0_stddev_slip, 2.0); + + // initialize motion model + motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, slip, slip_cov, length); } - return true; + /* calc nearest corner index*/ + setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step } -bool NormalVehicleTracker::measureWithPose( - const autoware_auto_perception_msgs::msg::DetectedObject & object) +bool NormalVehicleTracker::predict(const rclcpp::Time & time) { - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; - - float r_cov_x; - float r_cov_y; - const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - - if (label == Label::CAR) { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; - } else if (utils::isLargeVehicleLabel(label)) { - constexpr float r_stddev_x = 2.0; // [m] - constexpr float r_stddev_y = 2.0; // [m] - r_cov_x = std::pow(r_stddev_x, 2.0); - r_cov_y = std::pow(r_stddev_y, 2.0); - } else { - r_cov_x = ekf_params_.r_cov_x; - r_cov_y = ekf_params_.r_cov_y; - } - - // Decide dimension of measurement vector - bool enable_velocity_measurement = false; - if (object.kinematics.has_twist) { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); - const double predicted_vel = X_t(IDX::VEL); - const double observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; + return motion_model_.predictState(time); +} - if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { - // Velocity deviation is small - enable_velocity_measurement = true; - } - } +autoware_auto_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) +{ + autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; - // pos x, pos y, yaw, vel depending on pose measurement - const int dim_y = enable_velocity_measurement ? 4 : 3; - double measurement_yaw = getMeasurementYaw(object); // get sign-solved yaw angle - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); + // current (predicted) state + const double tracked_x = motion_model_.getStateElement(IDX::X); + const double tracked_y = motion_model_.getStateElement(IDX::Y); + const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); - // convert to boundingbox if input is convex shape + // OBJECT SHAPE MODEL + // convert to bounding box if input is convex shape autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { utils::convertConvexHullToBoundingBox(object, bbox_object); @@ -356,108 +196,134 @@ bool NormalVehicleTracker::measureWithPose( bbox_object = object; } - /* get offset measurement*/ - autoware_auto_perception_msgs::msg::DetectedObject offset_object; + // get offset measurement + int nearest_corner_index = utils::getNearestCornerOrSurface( + tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); utils::calcAnchorPointOffset( - last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, - bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); - - // Set measurement matrix C and observation vector Y - Eigen::MatrixXd Y(dim_y, 1); - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x; - Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y; - Y(IDX::YAW, 0) = measurement_yaw; - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - C(2, IDX::YAW) = 1.0; // for yaw - - // Set noise covariance matrix R - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(measurement_yaw); - const double sin_yaw = std::sin(measurement_yaw); - const double sin_2yaw = std::sin(2.0f * measurement_yaw); - R(0, 0) = r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - R(0, 1) = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - R(1, 1) = r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - R(1, 0) = R(0, 1); // y - x - R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw - } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; - R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; - R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; - R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - } - - // Update the velocity when necessary - if (dim_y == 4) { - Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; - C(3, IDX::VEL) = 1.0; // for vel + last_input_bounding_box_.width, last_input_bounding_box_.length, nearest_corner_index, + bbox_object, tracked_yaw, updating_object, tracking_offset_); - if (!object.kinematics.has_twist_covariance) { - R(3, 3) = ekf_params_.r_cov_vel; // vel -vel + // UNCERTAINTY MODEL + if (!object.kinematics.has_position_covariance) { + // measurement noise covariance + float r_cov_x; + float r_cov_y; + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); + if (label == Label::CAR) { + r_cov_x = ekf_params_.r_cov_x; + r_cov_y = ekf_params_.r_cov_y; + } else if (utils::isLargeVehicleLabel(label)) { + // if label is changed, enlarge the measurement noise covariance + constexpr float r_stddev_x = 2.0; // [m] + constexpr float r_stddev_y = 2.0; // [m] + r_cov_x = std::pow(r_stddev_x, 2.0); + r_cov_y = std::pow(r_stddev_y, 2.0); } else { - R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + r_cov_x = ekf_params_.r_cov_x; + r_cov_y = ekf_params_.r_cov_y; + } + + // yaw angle fix + double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + bool is_yaw_available = + object.kinematics.orientation_availability != + autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + + // fill covariance matrix + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + const double cos_yaw = std::cos(pose_yaw); + const double sin_yaw = std::sin(pose_yaw); + const double sin_2yaw = std::sin(2.0f * pose_yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[utils::MSG_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; // y - x + pose_cov[utils::MSG_COV_IDX::X_YAW] = 0.0; // x - yaw + pose_cov[utils::MSG_COV_IDX::Y_YAW] = 0.0; // y - yaw + pose_cov[utils::MSG_COV_IDX::YAW_X] = 0.0; // yaw - x + pose_cov[utils::MSG_COV_IDX::YAW_Y] = 0.0; // yaw - y + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = ekf_params_.r_cov_yaw; // yaw - yaw + if (!is_yaw_available) { + pose_cov[utils::MSG_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value } + auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; + twist_cov[utils::MSG_COV_IDX::X_X] = ekf_params_.r_cov_vel; // vel - vel } - // ekf update - if (!ekf_.update(Y, C, R)) { - RCLCPP_WARN(logger_, "Cannot update"); + return updating_object; +} + +bool NormalVehicleTracker::measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + // current (predicted) state + const double tracked_vel = motion_model_.getStateElement(IDX::VEL); + + // velocity capability is checked only when the object has velocity measurement + // and the predicted velocity is close to the observed velocity + bool is_velocity_available = false; + if (object.kinematics.has_twist) { + const double & observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; + if (std::fabs(tracked_vel - observed_vel) < velocity_deviation_threshold_) { + // Velocity deviation is small + is_velocity_available = true; + } } - // normalize yaw and limit vel, slip + // update + bool is_updated = false; { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { - X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; - } - if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { - X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double vel = object.kinematics.twist_with_covariance.twist.linear.x; + + if (is_velocity_available) { + is_updated = motion_model_.updateStatePoseHeadVel( + x, y, yaw, object.kinematics.pose_with_covariance.covariance, vel, + object.kinematics.twist_with_covariance.covariance); + } else { + is_updated = motion_model_.updateStatePoseHead( + x, y, yaw, object.kinematics.pose_with_covariance.covariance); } - ekf_.init(X_t, P_t); + motion_model_.limitStates(); } // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + constexpr double gain = 0.1; + z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; - return true; + return is_updated; } bool NormalVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - // if the input shape is convex type, convert it to bbox type - autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - utils::convertConvexHullToBoundingBox(object, bbox_object); - } else { - bbox_object = object; - } + constexpr double gain = 0.1; + constexpr double gain_inv = 1.0 - gain; - constexpr float gain = 0.9; - bounding_box_.length = - gain * bounding_box_.length + (1.0 - gain) * bbox_object.shape.dimensions.x; - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * bbox_object.shape.dimensions.y; - bounding_box_.height = - gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; + // update object size + bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; + bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; + bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; last_input_bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); - // update lf, lr - lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m - lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m + // update motion model + motion_model_.updateExtendedState(bounding_box_.length); + + // // update offset into object position + // motion_model_.adjustPosition(gain * tracking_offset_.x(), gain * tracking_offset_.y()); + // // update offset + // tracking_offset_.x() = gain_inv * tracking_offset_.x(); + // tracking_offset_.y() = gain_inv * tracking_offset_.y(); return true; } @@ -466,20 +332,30 @@ bool NormalVehicleTracker::measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { - const auto & current_classification = getClassification(); + // keep the latest input object object_ = object; + + // update classification + const auto & current_classification = getClassification(); if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + // check time gap + const double dt = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < dt) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + logger_, + "NormalVehicleTracker::measure There is a large gap between predicted time and measurement " + "time. (%f)", + dt); } - measureWithPose(object); - measureWithShape(object); + // update object + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = + getUpdatingObject(object, self_transform); + measureWithPose(updating_object); + measureWithShape(updating_object); /* calc nearest corner index*/ setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step @@ -494,100 +370,28 @@ bool NormalVehicleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict state - KalmanFilter tmp_ekf_for_no_update = ekf_; - const double dt = (time - last_update_time_).seconds(); - if (0.001 /*1msec*/ < dt) { - predict(dt, tmp_ekf_for_no_update); - } - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - Eigen::MatrixXd P(ekf_params_.dim_x, ekf_params_.dim_x); // predicted state - tmp_ekf_for_no_update.getX(X_t); - tmp_ekf_for_no_update.getP(P); - - /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + // predict from motion model + if (!motion_model_.getPredictedState( + time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, + twist_with_cov.covariance)) { + RCLCPP_WARN(logger_, "NormalVehicleTracker::getTrackedObject: Failed to get predicted state."); + return false; + } + // recover bounding box from tracking point const double dl = bounding_box_.length - last_input_bounding_box_.length; const double dw = bounding_box_.width - last_input_bounding_box_.width; const Eigen::Vector2d recovered_pose = utils::recoverFromTrackingPoint( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); - X_t(IDX::X) = recovered_pose.x(); - X_t(IDX::Y) = recovered_pose.y(); + pose_with_cov.pose.position.x, pose_with_cov.pose.position.y, + motion_model_.getStateElement(IDX::YAW), dw, dl, last_nearest_corner_index_, tracking_offset_); + pose_with_cov.pose.position.x = recovered_pose.x(); + pose_with_cov.pose.position.y = recovered_pose.y(); // position - pose_with_cov.pose.position.x = X_t(IDX::X); - pose_with_cov.pose.position.y = X_t(IDX::Y); pose_with_cov.pose.position.z = z_; - // quaternion - { - double roll, pitch, yaw; - tf2::Quaternion original_quaternion; - tf2::fromMsg(object_.kinematics.pose_with_covariance.pose.orientation, original_quaternion); - tf2::Matrix3x3(original_quaternion).getRPY(roll, pitch, yaw); - tf2::Quaternion filtered_quaternion; - filtered_quaternion.setRPY(roll, pitch, X_t(IDX::YAW)); - pose_with_cov.pose.orientation.x = filtered_quaternion.x(); - pose_with_cov.pose.orientation.y = filtered_quaternion.y(); - pose_with_cov.pose.orientation.z = filtered_quaternion.z(); - pose_with_cov.pose.orientation.w = filtered_quaternion.w(); - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; - } - // position covariance - constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double p_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = r_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); - - // twist - twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); - twist_with_cov.twist.angular.z = - X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r - /* twist covariance - * convert covariance from velocity, slip angle to vx, vy, and yaw angle - * - * vx = vel * cos(slip) - * vy = vel * sin(slip) - * wz = vel * sin(slip) / l_r = vy / l_r - * - */ - - constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - - Eigen::MatrixXd cov_jacob(3, 2); - cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), - std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; - Eigen::MatrixXd cov_twist(2, 2); - cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), - P(IDX::SLIP, IDX::SLIP); - Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); - - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; // set shape object.shape.dimensions.x = bounding_box_.length; @@ -600,32 +404,3 @@ bool NormalVehicleTracker::getTrackedObject( return true; } - -void NormalVehicleTracker::setNearestCornerOrSurfaceIndex( - const geometry_msgs::msg::Transform & self_transform) -{ - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); - last_nearest_corner_index_ = utils::getNearestCornerOrSurface( - X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, - self_transform); -} - -double NormalVehicleTracker::getMeasurementYaw( - const autoware_auto_perception_msgs::msg::DetectedObject & object) -{ - double measurement_yaw = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - ekf_.getX(X_t); - // Fixed measurement_yaw to be in the range of +-90 degrees of X_t(IDX::YAW) - while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - measurement_yaw = measurement_yaw + M_PI; - } - while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - measurement_yaw = measurement_yaw - M_PI; - } - } - return measurement_yaw; -} diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp index eed9d05359b77..d61a9a02ccd80 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -43,7 +43,7 @@ bool PedestrianAndBicycleTracker::measure( pedestrian_tracker_.measure(object, time, self_transform); bicycle_tracker_.measure(object, time, self_transform); if (object_recognition_utils::getHighestProbLabel(object.classification) != Label::UNKNOWN) - setClassification(object.classification); + updateClassification(object.classification); return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index 7562684b84220..e1c07388836f6 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -47,7 +47,6 @@ PedestrianTracker::PedestrianTracker( const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("PedestrianTracker")), - last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; @@ -60,80 +59,8 @@ PedestrianTracker::PedestrianTracker( ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); - // initial state covariance - float p0_stddev_x = 2.0; // [m] - float p0_stddev_y = 2.0; // [m] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(120); // [m/s] - float p0_stddev_wz = tier4_autoware_utils::deg2rad(360); // [rad/s] - ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); - ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); - ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); - ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0); - // process noise covariance - float q_stddev_x = 0.4; // [m/s] - float q_stddev_y = 0.4; // [m/s] - float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)] - float q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] - ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); - ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); - // limitations - max_vx_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s] - - // initialize state vector X - Eigen::MatrixXd X(ekf_params_.dim_x, 1); - X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; - X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; - X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - if (object.kinematics.has_twist) { - X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; - X(IDX::WZ) = object.kinematics.twist_with_covariance.twist.angular.z; - } else { - X(IDX::VX) = 0.0; - X(IDX::WZ) = 0.0; - } - - // initialize state covariance matrix P - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - if (!object.kinematics.has_position_covariance) { - const double cos_yaw = std::cos(X(IDX::YAW)); - const double sin_yaw = std::sin(X(IDX::YAW)); - const double sin_2yaw = std::sin(2.0f * X(IDX::YAW)); - // Rotate the covariance matrix according to the vehicle yaw - // because p0_cov_x and y are in the vehicle coordinate system. - P(IDX::X, IDX::X) = - ekf_params_.p0_cov_x * cos_yaw * cos_yaw + ekf_params_.p0_cov_y * sin_yaw * sin_yaw; - P(IDX::X, IDX::Y) = 0.5f * (ekf_params_.p0_cov_x - ekf_params_.p0_cov_y) * sin_2yaw; - P(IDX::Y, IDX::Y) = - ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; - P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); - P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; - } else { - P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - P(IDX::YAW, IDX::YAW) = - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::WZ, IDX::WZ) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; - } - } + // OBJECT SHAPE MODEL bounding_box_ = {0.5, 0.5, 1.7}; cylinder_ = {0.3, 1.7}; if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -142,181 +69,159 @@ PedestrianTracker::PedestrianTracker( } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { cylinder_ = {object.shape.dimensions.x, object.shape.dimensions.z}; } + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); + cylinder_.width = std::max(cylinder_.width, 0.3); + cylinder_.height = std::max(cylinder_.height, 0.3); + + // Set motion model parameters + { + constexpr double q_stddev_x = 0.5; // [m/s] + constexpr double q_stddev_y = 0.5; // [m/s] + constexpr double q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] + constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] + constexpr double q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] + motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vx, q_stddev_wz); + } + + // Set motion limits + motion_model_.setMotionLimits( + tier4_autoware_utils::kmph2mps(100), /* [m/s] maximum velocity */ + 30.0 /* [deg/s] maximum turn rate */ + ); + + // Set initial state + { + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + auto pose_cov = object.kinematics.pose_with_covariance.covariance; + double vel = 0.0; + double wz = 0.0; + double vel_cov; + double wz_cov; + + if (object.kinematics.has_twist) { + vel = object.kinematics.twist_with_covariance.twist.linear.x; + wz = object.kinematics.twist_with_covariance.twist.angular.z; + } + + if (!object.kinematics.has_position_covariance) { + // initial state covariance + constexpr double p0_stddev_x = 2.0; // in object coordinate [m] + constexpr double p0_stddev_y = 2.0; // in object coordinate [m] + constexpr double p0_stddev_yaw = + tier4_autoware_utils::deg2rad(1000); // in map coordinate [rad] + constexpr double p0_cov_x = std::pow(p0_stddev_x, 2.0); + constexpr double p0_cov_y = std::pow(p0_stddev_y, 2.0); + constexpr double p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + const double sin_2yaw = std::sin(2.0 * yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[utils::MSG_COV_IDX::Y_Y] = + p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = p0_cov_yaw; + } + + if (!object.kinematics.has_twist_covariance) { + constexpr double p0_stddev_vel = + tier4_autoware_utils::kmph2mps(120); // in object coordinate [m/s] + constexpr double p0_stddev_wz = + tier4_autoware_utils::deg2rad(360); // in object coordinate [rad/s] + vel_cov = std::pow(p0_stddev_vel, 2.0); + wz_cov = std::pow(p0_stddev_wz, 2.0); + } else { + vel_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + wz_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; + } - ekf_.init(X, P); + // initialize motion model + motion_model_.initialize(time, x, y, yaw, pose_cov, vel, vel_cov, wz, wz_cov); + } } bool PedestrianTracker::predict(const rclcpp::Time & time) { - const double dt = (time - last_update_time_).seconds(); - bool ret = predict(dt, ekf_); - if (ret) { - last_update_time_ = time; - } - return ret; + return motion_model_.predictState(time); } -bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const +autoware_auto_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & /*self_transform*/) { - // cspell: ignore CTRV - /* Motion model: Constant turn-rate motion model (CTRV) - * - * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt - * yaw_{k+1} = yaw_k + (wz_k) * dt - * vx_{k+1} = vx_k - * wz_{k+1} = wz_k - * - */ - - /* Jacobian Matrix - * - * A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0] - * [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0] - * [ 0, 0, 1, 0, dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] - */ - - // Current state vector X t - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf.getX(X_t); - const double cos_yaw = std::cos(X_t(IDX::YAW)); - const double sin_yaw = std::sin(X_t(IDX::YAW)); - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - - // Predict state vector X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VX) * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + (X_t(IDX::WZ)) * dt; // dyaw = omega - X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::WZ) = X_t(IDX::WZ); - - // State transition matrix A - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -X_t(IDX::VX) * sin_yaw * dt; - A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::Y, IDX::YAW) = X_t(IDX::VX) * cos_yaw * dt; - A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::YAW, IDX::WZ) = dt; - - // Process noise covariance Q - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Rotate the covariance matrix according to the vehicle yaw - // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = - (ekf_params_.q_cov_x * cos_yaw * cos_yaw + ekf_params_.q_cov_y * sin_yaw * sin_yaw) * dt * dt; - Q(IDX::X, IDX::Y) = (0.5f * (ekf_params_.q_cov_x - ekf_params_.q_cov_y) * sin_2yaw) * dt * dt; - Q(IDX::Y, IDX::Y) = - (ekf_params_.q_cov_x * sin_yaw * sin_yaw + ekf_params_.q_cov_y * cos_yaw * cos_yaw) * dt * dt; - Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; - Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::WZ, IDX::WZ) = ekf_params_.q_cov_wz * dt * dt; - Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); - - if (!ekf.predict(X_next_t, A, Q)) { - RCLCPP_WARN(logger_, "Cannot predict"); - } + autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; - return true; + // UNCERTAINTY MODEL + if (!object.kinematics.has_position_covariance) { + const double & r_cov_x = ekf_params_.r_cov_x; + const double & r_cov_y = ekf_params_.r_cov_y; + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double cos_yaw = std::cos(pose_yaw); + const double sin_yaw = std::sin(pose_yaw); + const double sin_2yaw = std::sin(2.0f * pose_yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[utils::MSG_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; // y - x + } + return updating_object; } bool PedestrianTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - constexpr int dim_y = 2; // pos x, pos y depending on Pose output - // double measurement_yaw = - // tier4_autoware_utils::normalizeRadian(tf2::getYaw(object.state.pose_covariance.pose.orientation)); - // { - // Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - // ekf_.getX(X_t); - // while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - // measurement_yaw = measurement_yaw + M_PI; - // } - // while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - // measurement_yaw = measurement_yaw - M_PI; - // } - // float theta = std::acos( - // std::cos(X_t(IDX::YAW)) * std::cos(measurement_yaw) + - // std::sin(X_t(IDX::YAW)) * std::sin(measurement_yaw)); - // if (tier4_autoware_utils::deg2rad(60) < std::fabs(theta)) return false; - // } - - // Set measurement matrix C and observation vector Y - Eigen::MatrixXd Y(dim_y, 1); - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Y << object.kinematics.pose_with_covariance.pose.position.x, - object.kinematics.pose_with_covariance.pose.position.y; - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - // C(2, IDX::YAW) = 1.0; // for yaw - - // Set noise covariance matrix R - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { - R(0, 0) = ekf_params_.r_cov_x; // x - x - R(0, 1) = 0.0; // x - y - R(1, 1) = ekf_params_.r_cov_y; // y - y - R(1, 0) = R(0, 1); // y - x - // R(2, 2) = ekf_params_.r_cov_yaw; // yaw - yaw - } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - // R(0, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_YAW]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - // R(1, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_YAW]; - // R(2, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_X]; - // R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; - // R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; - } - - // ekf update - if (!ekf_.update(Y, C, R)) { - RCLCPP_WARN(logger_, "Cannot update"); - } - - // normalize yaw and limit vx, wz + // update motion model + bool is_updated = false; { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { - X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; - } - if (!(-max_wz_ <= X_t(IDX::WZ) && X_t(IDX::WZ) <= max_wz_)) { - X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -max_wz_ : max_wz_; - } - ekf_.init(X_t, P_t); + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + + is_updated = + motion_model_.updateStatePose(x, y, object.kinematics.pose_with_covariance.covariance); + motion_model_.limitStates(); } // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + constexpr double gain = 0.1; + z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; - return true; + return is_updated; } bool PedestrianTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - constexpr float gain = 0.9; + constexpr double gain = 0.1; + constexpr double gain_inv = 1.0 - gain; + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.x; - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.y; - bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; + bounding_box_.length = gain_inv * bounding_box_.length + gain * object.shape.dimensions.x; + bounding_box_.width = gain_inv * bounding_box_.width + gain * object.shape.dimensions.y; + bounding_box_.height = gain_inv * bounding_box_.height + gain * object.shape.dimensions.z; } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { - cylinder_.width = gain * cylinder_.width + (1.0 - gain) * object.shape.dimensions.x; - cylinder_.height = gain * cylinder_.height + (1.0 - gain) * object.shape.dimensions.z; + cylinder_.width = gain_inv * cylinder_.width + gain * object.shape.dimensions.x; + cylinder_.height = gain_inv * cylinder_.height + gain * object.shape.dimensions.z; } else { return false; } + // set minimum size + bounding_box_.length = std::max(bounding_box_.length, 0.3); + bounding_box_.width = std::max(bounding_box_.width, 0.3); + bounding_box_.height = std::max(bounding_box_.height, 0.3); + cylinder_.width = std::max(cylinder_.width, 0.3); + cylinder_.height = std::max(cylinder_.height, 0.3); + return true; } @@ -324,20 +229,29 @@ bool PedestrianTracker::measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { - const auto & current_classification = getClassification(); + // keep the latest input object object_ = object; + + const auto & current_classification = getClassification(); if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { setClassification(current_classification); } - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + // check time gap + const double dt = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < dt) { RCLCPP_WARN( - logger_, "There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + logger_, + "PedestrianTracker::measure There is a large gap between predicted time and measurement " + "time. (%f)", + dt); } - measureWithPose(object); - measureWithShape(object); + // update object + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = + getUpdatingObject(object, self_transform); + measureWithPose(updating_object); + measureWithShape(updating_object); (void)self_transform; // currently do not use self vehicle position return true; @@ -350,70 +264,19 @@ bool PedestrianTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict state - KalmanFilter tmp_ekf_for_no_update = ekf_; - const double dt = (time - last_update_time_).seconds(); - if (0.001 /*1msec*/ < dt) { - predict(dt, tmp_ekf_for_no_update); - } - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - Eigen::MatrixXd P(ekf_params_.dim_x, ekf_params_.dim_x); // predicted state - tmp_ekf_for_no_update.getX(X_t); - tmp_ekf_for_no_update.getP(P); - - /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + // predict from motion model + if (!motion_model_.getPredictedState( + time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, + twist_with_cov.covariance)) { + RCLCPP_WARN(logger_, "PedestrianTracker::getTrackedObject: Failed to get predicted state."); + return false; + } + // position - pose_with_cov.pose.position.x = X_t(IDX::X); - pose_with_cov.pose.position.y = X_t(IDX::Y); pose_with_cov.pose.position.z = z_; - // quaternion - { - double roll, pitch, yaw; - tf2::Quaternion original_quaternion; - tf2::fromMsg(object_.kinematics.pose_with_covariance.pose.orientation, original_quaternion); - tf2::Matrix3x3(original_quaternion).getRPY(roll, pitch, yaw); - tf2::Quaternion filtered_quaternion; - filtered_quaternion.setRPY(roll, pitch, X_t(IDX::YAW)); - pose_with_cov.pose.orientation.x = filtered_quaternion.x(); - pose_with_cov.pose.orientation.y = filtered_quaternion.y(); - pose_with_cov.pose.orientation.z = filtered_quaternion.z(); - pose_with_cov.pose.orientation.w = filtered_quaternion.w(); - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; - } - // position covariance - constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double p_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = r_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); - - // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX); - twist_with_cov.twist.angular.z = X_t(IDX::WZ); - // twist covariance - constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::WZ); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::WZ, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); // set shape if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp index ba684e4777947..a3320ff54afcb 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp @@ -54,6 +54,67 @@ bool Tracker::updateWithoutMeasurement() return true; } +void Tracker::updateClassification( + const std::vector & classification) +{ + // classification algorithm: + // 0. Normalize the input classification + // 1-1. Update the matched classification probability with a gain (ratio of 0.05) + // 1-2. If the label is not found, add it to the classification list + // 2. Remove the class with probability < remove_threshold (0.001) + // 3. Normalize tracking classification + + // Parameters + // if the remove_threshold is too high (compare to the gain), the classification will be removed + // immediately + const double gain = 0.05; + constexpr double remove_threshold = 0.001; + + // Normalization function + auto normalizeProbabilities = + [](std::vector & classification) { + double sum = 0.0; + for (const auto & class_ : classification) { + sum += class_.probability; + } + for (auto & class_ : classification) { + class_.probability /= sum; + } + }; + + // Normalize the input + auto classification_input = classification; + normalizeProbabilities(classification_input); + + // Update the matched classification probability with a gain + for (const auto & new_class : classification_input) { + bool found = false; + for (auto & old_class : classification_) { + if (new_class.label == old_class.label) { + old_class.probability += new_class.probability * gain; + found = true; + break; + } + } + // If the label is not found, add it to the classification list + if (!found) { + auto adding_class = new_class; + adding_class.probability *= gain; + classification_.push_back(adding_class); + } + } + + // If the probability is less than the threshold, remove the class + classification_.erase( + std::remove_if( + classification_.begin(), classification_.end(), + [remove_threshold](const auto & class_) { return class_.probability < remove_threshold; }), + classification_.end()); + + // Normalize tracking classification + normalizeProbabilities(classification_); +} + geometry_msgs::msg::PoseWithCovariance Tracker::getPoseWithCovariance( const rclcpp::Time & time) const { diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index 897b858a6aabe..450bb2d94abb6 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -12,6 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "multi_object_tracker/tracker/model/unknown_tracker.hpp" + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include + #include #include #include @@ -22,227 +30,179 @@ #else #include #endif - -#define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/unknown_tracker.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" +#define EIGEN_MPL2_ONLY #include #include -#include -#include -#include UnknownTracker::UnknownTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("UnknownTracker")), - last_update_time_(time), z_(object.kinematics.pose_with_covariance.pose.position.z) { object_ = object; // initialize params - float q_stddev_x = 0.0; // [m/s] - float q_stddev_y = 0.0; // [m/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(20); // [m/(s*s)] - float q_stddev_vy = tier4_autoware_utils::kmph2mps(20); // [m/(s*s)] - float r_stddev_x = 1.0; // [m] - float r_stddev_y = 1.0; // [m] - float p0_stddev_x = 1.0; // [m/s] - float p0_stddev_y = 1.0; // [m/s] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float p0_stddev_vy = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_vy = std::pow(q_stddev_vy, 2.0); + // measurement noise covariance + constexpr double r_stddev_x = 1.0; // [m] + constexpr double r_stddev_y = 1.0; // [m] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); - ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); - ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); - ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); - ekf_params_.p0_cov_vy = std::pow(p0_stddev_vy, 2.0); - max_vx_ = tier4_autoware_utils::kmph2mps(60); // [m/s] - max_vy_ = tier4_autoware_utils::kmph2mps(60); // [m/s] - - // initialize X matrix - Eigen::MatrixXd X(ekf_params_.dim_x, 1); - X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; - X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; - if (object.kinematics.has_twist) { - X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; - X(IDX::VY) = object.kinematics.twist_with_covariance.twist.linear.y; - } else { - X(IDX::VX) = 0.0; - X(IDX::VY) = 0.0; + + // Set motion model parameters + { + constexpr double q_stddev_x = 0.5; // [m/s] + constexpr double q_stddev_y = 0.5; // [m/s] + constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] + constexpr double q_stddev_vy = 9.8 * 0.3; // [m/(s*s)] + motion_model_.setMotionParams(q_stddev_x, q_stddev_y, q_stddev_vx, q_stddev_vy); } - // initialize P matrix - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - if (!object.kinematics.has_position_covariance) { - // Rotate the covariance matrix according to the vehicle yaw - // because p0_cov_x and y are in the vehicle coordinate system. - P(IDX::X, IDX::X) = ekf_params_.p0_cov_x; - P(IDX::X, IDX::Y) = 0.0; - P(IDX::Y, IDX::Y) = ekf_params_.p0_cov_y; - P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::VY, IDX::VY) = ekf_params_.p0_cov_vy; - } else { - P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - P(IDX::Y, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P(IDX::Y, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - P(IDX::VY, IDX::VY) = - object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; - P(IDX::VY, IDX::VY) = ekf_params_.p0_cov_vy; + // Set motion limits + motion_model_.setMotionLimits( + tier4_autoware_utils::kmph2mps(60), /* [m/s] maximum velocity, x */ + tier4_autoware_utils::kmph2mps(60) /* [m/s] maximum velocity, y */ + ); + + // Set initial state + { + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + auto pose_cov = object.kinematics.pose_with_covariance.covariance; + auto twist_cov = object.kinematics.twist_with_covariance.covariance; + const double yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + + double vx = 0.0; + double vy = 0.0; + if (object.kinematics.has_twist) { + const double & vel_x = object.kinematics.twist_with_covariance.twist.linear.x; + const double & vel_y = object.kinematics.twist_with_covariance.twist.linear.y; + vx = std::cos(yaw) * vel_x - std::sin(yaw) * vel_y; + vy = std::sin(yaw) * vel_x + std::cos(yaw) * vel_y; + } + + if (!object.kinematics.has_position_covariance) { + constexpr double p0_stddev_x = 1.0; // [m] + constexpr double p0_stddev_y = 1.0; // [m] + + const double p0_cov_x = std::pow(p0_stddev_x, 2.0); + const double p0_cov_y = std::pow(p0_stddev_y, 2.0); + + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + const double sin_2yaw = std::sin(2.0 * yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + p0_cov_x * cos_yaw * cos_yaw + p0_cov_y * sin_yaw * sin_yaw; + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5 * (p0_cov_x - p0_cov_y) * sin_2yaw; + pose_cov[utils::MSG_COV_IDX::Y_Y] = + p0_cov_x * sin_yaw * sin_yaw + p0_cov_y * cos_yaw * cos_yaw; + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; + } + + if (!object.kinematics.has_twist_covariance) { + constexpr double p0_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/s] + constexpr double p0_stddev_vy = tier4_autoware_utils::kmph2mps(10); // [m/s] + const double p0_cov_vx = std::pow(p0_stddev_vx, 2.0); + const double p0_cov_vy = std::pow(p0_stddev_vy, 2.0); + twist_cov[utils::MSG_COV_IDX::X_X] = p0_cov_vx; + twist_cov[utils::MSG_COV_IDX::X_Y] = 0.0; + twist_cov[utils::MSG_COV_IDX::Y_X] = 0.0; + twist_cov[utils::MSG_COV_IDX::Y_Y] = p0_cov_vy; } - } - ekf_.init(X, P); + // rotate twist covariance matrix, since it is in the vehicle coordinate system + Eigen::MatrixXd twist_cov_rotate(2, 2); + twist_cov_rotate(0, 0) = twist_cov[utils::MSG_COV_IDX::X_X]; + twist_cov_rotate(0, 1) = twist_cov[utils::MSG_COV_IDX::X_Y]; + twist_cov_rotate(1, 0) = twist_cov[utils::MSG_COV_IDX::Y_X]; + twist_cov_rotate(1, 1) = twist_cov[utils::MSG_COV_IDX::Y_Y]; + Eigen::MatrixXd R_yaw = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); + Eigen::MatrixXd twist_cov_rotated = R_yaw * twist_cov_rotate * R_yaw.transpose(); + twist_cov[utils::MSG_COV_IDX::X_X] = twist_cov_rotated(0, 0); + twist_cov[utils::MSG_COV_IDX::X_Y] = twist_cov_rotated(0, 1); + twist_cov[utils::MSG_COV_IDX::Y_X] = twist_cov_rotated(1, 0); + twist_cov[utils::MSG_COV_IDX::Y_Y] = twist_cov_rotated(1, 1); + + // initialize motion model + motion_model_.initialize(time, x, y, pose_cov, vx, vy, twist_cov); + } } bool UnknownTracker::predict(const rclcpp::Time & time) { - const double dt = (time - last_update_time_).seconds(); - bool ret = predict(dt, ekf_); - if (ret) { - last_update_time_ = time; - } - return ret; + return motion_model_.predictState(time); } -bool UnknownTracker::predict(const double dt, KalmanFilter & ekf) const +autoware_auto_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & /*self_transform*/) { - /* == Nonlinear model == - * - * x_{k+1} = x_k + vx_k * dt - * y_{k+1} = y_k + vx_k * dt - * vx_{k+1} = vx_k - * vy_{k+1} = vy_k - * - */ - - /* == Linearized model == - * - * A = [ 1, 0, dt, 0] - * [ 0, 1, 0, dt] - * [ 0, 0, 1, 0] - * [ 0, 0, 0, 1] - */ - - // X t - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf.getX(X_t); - - // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * dt; - X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VY) * dt; - X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::VY) = X_t(IDX::VY); - - // A - Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::VX) = dt; - A(IDX::Y, IDX::VY) = dt; - - // Q - Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - // Rotate the covariance matrix according to the vehicle yaw - // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = ekf_params_.q_cov_x * dt * dt; - Q(IDX::X, IDX::Y) = 0.0; - Q(IDX::Y, IDX::Y) = ekf_params_.q_cov_y * dt * dt; - Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::VY, IDX::VY) = ekf_params_.q_cov_vy * dt * dt; - Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); - - if (!ekf.predict(X_next_t, A, Q)) { - RCLCPP_WARN(logger_, "Pedestrian : Cannot predict"); - } + autoware_auto_perception_msgs::msg::DetectedObject updating_object = object; - return true; + // UNCERTAINTY MODEL + if (!object.kinematics.has_position_covariance) { + const double & r_cov_x = ekf_params_.r_cov_x; + const double & r_cov_y = ekf_params_.r_cov_y; + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double cos_yaw = std::cos(pose_yaw); + const double sin_yaw = std::sin(pose_yaw); + const double sin_2yaw = std::sin(2.0f * pose_yaw); + pose_cov[utils::MSG_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[utils::MSG_COV_IDX::X_Y] = 0.5f * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[utils::MSG_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[utils::MSG_COV_IDX::Y_X] = pose_cov[utils::MSG_COV_IDX::X_Y]; // y - x + } + return updating_object; } bool UnknownTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { - constexpr int dim_y = 2; // pos x, pos y depending on Pose output - - /* Set measurement matrix */ - Eigen::MatrixXd Y(dim_y, 1); - Y << object.kinematics.pose_with_covariance.pose.position.x, - object.kinematics.pose_with_covariance.pose.position.y; - - /* Set measurement matrix */ - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - - /* Set measurement noise covariance */ - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { - R(0, 0) = ekf_params_.r_cov_x; // x - x - R(0, 1) = 0.0; // x - y - R(1, 1) = ekf_params_.r_cov_y; // y - y - R(1, 0) = R(0, 1); // y - x - } else { - R(0, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; - R(0, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y]; - R(1, 0) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X]; - R(1, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - } - if (!ekf_.update(Y, C, R)) { - RCLCPP_WARN(logger_, "Pedestrian : Cannot update"); - } - - // limit vx, vy + // update motion model + bool is_updated = false; { - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { - X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; - } - if (!(-max_vy_ <= X_t(IDX::VY) && X_t(IDX::VY) <= max_vy_)) { - X_t(IDX::VY) = X_t(IDX::VY) < 0 ? -max_vy_ : max_vy_; - } - ekf_.init(X_t, P_t); + const double x = object.kinematics.pose_with_covariance.pose.position.x; + const double y = object.kinematics.pose_with_covariance.pose.position.y; + + is_updated = + motion_model_.updateStatePose(x, y, object.kinematics.pose_with_covariance.covariance); + motion_model_.limitStates(); } // position z - constexpr float gain = 0.9; - z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + constexpr double gain = 0.1; + z_ = (1.0 - gain) * z_ + gain * object.kinematics.pose_with_covariance.pose.position.z; - return true; + return is_updated; } bool UnknownTracker::measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & /*self_transform*/) + const geometry_msgs::msg::Transform & self_transform) { + // keep the latest input object object_ = object; - if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + // check time gap + const double dt = motion_model_.getDeltaTime(time); + if (0.01 /*10msec*/ < dt) { RCLCPP_WARN( logger_, - "Pedestrian : There is a large gap between predicted time and measurement time. (%f)", - (time - last_update_time_).seconds()); + "UnknownTracker::measure There is a large gap between predicted time and measurement time. " + "(%f)", + dt); } - measureWithPose(object); + // update object + const autoware_auto_perception_msgs::msg::DetectedObject updating_object = + getUpdatingObject(object, self_transform); + measureWithPose(updating_object); return true; } @@ -254,54 +214,19 @@ bool UnknownTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict kinematics - KalmanFilter tmp_ekf_for_no_update = ekf_; - const double dt = (time - last_update_time_).seconds(); - if (0.001 /*1msec*/ < dt) { - predict(dt, tmp_ekf_for_no_update); - } - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - Eigen::MatrixXd P(ekf_params_.dim_x, ekf_params_.dim_x); // predicted state - tmp_ekf_for_no_update.getX(X_t); - tmp_ekf_for_no_update.getP(P); - auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; + + // predict from motion model + if (!motion_model_.getPredictedState( + time, pose_with_cov.pose, pose_with_cov.covariance, twist_with_cov.twist, + twist_with_cov.covariance)) { + RCLCPP_WARN(logger_, "UnknownTracker::getTrackedObject: Failed to get predicted state."); + return false; + } + // position - pose_with_cov.pose.position.x = X_t(IDX::X); - pose_with_cov.pose.position.y = X_t(IDX::Y); pose_with_cov.pose.position.z = z_; - constexpr double z_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double r_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double p_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double yaw_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = r_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = p_cov; - pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = yaw_cov; - - // twist - const auto pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); - const double cos = std::cos(-pose_yaw); - const double sin = std::sin(-pose_yaw); - twist_with_cov.twist.linear.x = cos * X_t(IDX::VX) - sin * X_t(IDX::VY); - twist_with_cov.twist.linear.y = sin * X_t(IDX::VX) + cos * X_t(IDX::VY); - - // twist covariance - constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - constexpr double wz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::VY, IDX::VY); - twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = wz_cov; return true; } diff --git a/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp new file mode 100644 index 0000000000000..ef8a45f608098 --- /dev/null +++ b/perception/multi_object_tracker/src/tracker/motion_model/bicycle_motion_model.cpp @@ -0,0 +1,483 @@ +// 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. +// +// +// Author: v1.0 Taekjin Lee +// + +#include "multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" + +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +// cspell: ignore CTRV +// Bicycle CTRV motion model +// CTRV : Constant Turn Rate and constant Velocity + +BicycleMotionModel::BicycleMotionModel() +: MotionModel(), logger_(rclcpp::get_logger("BicycleMotionModel")) +{ + // Initialize motion parameters + setDefaultParams(); +} + +void BicycleMotionModel::setDefaultParams() +{ + // set default motion parameters + constexpr double q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + constexpr double q_stddev_yaw_rate_min = 1.5; // [deg/s] uncertain yaw change rate + constexpr double q_stddev_yaw_rate_max = 15.0; // [deg/s] uncertain yaw change rate + constexpr double q_stddev_slip_rate_min = 0.3; // [deg/s] uncertain slip angle change rate + constexpr double q_stddev_slip_rate_max = 10.0; // [deg/s] uncertain slip angle change rate + constexpr double q_max_slip_angle = 30.0; // [deg] max slip angle + // extended state parameters + constexpr double lf_ratio = 0.3; // 30% front from the center + constexpr double lf_min = 1.0; // minimum of 1.0m + constexpr double lr_ratio = 0.25; // 25% rear from the center + constexpr double lr_min = 1.0; // minimum of 1.0m + setMotionParams( + q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, + q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, + lr_min); + + // set motion limitations + constexpr double max_vel = tier4_autoware_utils::kmph2mps(100); // [m/s] maximum velocity + constexpr double max_slip = 30.0; // [deg] maximum slip angle + setMotionLimits(max_vel, max_slip); + + // set prediction parameters + constexpr double dt_max = 0.11; // [s] maximum time interval for prediction + setMaxDeltaTime(dt_max); +} + +void BicycleMotionModel::setMotionParams( + const double & q_stddev_acc_long, const double & q_stddev_acc_lat, + const double & q_stddev_yaw_rate_min, const double & q_stddev_yaw_rate_max, + const double & q_stddev_slip_rate_min, const double & q_stddev_slip_rate_max, + const double & q_max_slip_angle, const double & lf_ratio, const double & lf_min, + const double & lr_ratio, const double & lr_min) +{ + // set process noise covariance parameters + motion_params_.q_stddev_acc_long = q_stddev_acc_long; + motion_params_.q_stddev_acc_lat = q_stddev_acc_lat; + motion_params_.q_stddev_yaw_rate_min = tier4_autoware_utils::deg2rad(q_stddev_yaw_rate_min); + motion_params_.q_stddev_yaw_rate_max = tier4_autoware_utils::deg2rad(q_stddev_yaw_rate_max); + motion_params_.q_cov_slip_rate_min = + std::pow(tier4_autoware_utils::deg2rad(q_stddev_slip_rate_min), 2.0); + motion_params_.q_cov_slip_rate_max = + std::pow(tier4_autoware_utils::deg2rad(q_stddev_slip_rate_max), 2.0); + motion_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(q_max_slip_angle); + + constexpr double minimum_wheel_pos = 0.01; // minimum of 0.01m + if (lf_min < minimum_wheel_pos || lr_min < minimum_wheel_pos) { + RCLCPP_WARN( + logger_, + "BicycleMotionModel::setMotionParams: minimum wheel position should be greater than 0.01m."); + } + motion_params_.lf_min = (lf_min < minimum_wheel_pos) ? minimum_wheel_pos : lf_min; + motion_params_.lr_min = (lr_min < minimum_wheel_pos) ? minimum_wheel_pos : lr_min; + motion_params_.lf_ratio = lf_ratio; + motion_params_.lr_ratio = lr_ratio; +} + +void BicycleMotionModel::setMotionLimits(const double & max_vel, const double & max_slip) +{ + // set motion limitations + motion_params_.max_vel = max_vel; + motion_params_.max_slip = tier4_autoware_utils::deg2rad(max_slip); +} + +bool BicycleMotionModel::initialize( + const rclcpp::Time & time, const double & x, const double & y, const double & yaw, + const std::array & pose_cov, const double & vel, const double & vel_cov, + const double & slip, const double & slip_cov, const double & length) +{ + // initialize state vector X + Eigen::MatrixXd X(DIM, 1); + X << x, y, yaw, vel, slip; + + // initialize covariance matrix P + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); + P(IDX::X, IDX::X) = pose_cov[utils::MSG_COV_IDX::X_X]; + P(IDX::Y, IDX::Y) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + P(IDX::YAW, IDX::YAW) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + P(IDX::VEL, IDX::VEL) = vel_cov; + P(IDX::SLIP, IDX::SLIP) = slip_cov; + + // set initial extended state + if (!updateExtendedState(length)) return false; + + return MotionModel::initialize(time, X, P); +} + +bool BicycleMotionModel::updateStatePose( + const double & x, const double & y, const std::array & pose_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, without velocity + constexpr int DIM_Y = 2; + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + + return ekf_.update(Y, C, R); +} + +bool BicycleMotionModel::updateStatePoseHead( + const double & x, const double & y, const double & yaw, const std::array & pose_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, without velocity + constexpr int DIM_Y = 3; + + // fix yaw + double estimated_yaw = getStateElement(IDX::YAW); + double fixed_yaw = yaw; + double limiting_delta_yaw = M_PI_2; + while (std::fabs(estimated_yaw - fixed_yaw) > limiting_delta_yaw) { + if (fixed_yaw < estimated_yaw) { + fixed_yaw += 2 * limiting_delta_yaw; + } else { + fixed_yaw -= 2 * limiting_delta_yaw; + } + } + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y, fixed_yaw; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + C(2, IDX::YAW) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + + return ekf_.update(Y, C, R); +} + +bool BicycleMotionModel::updateStatePoseHeadVel( + const double & x, const double & y, const double & yaw, const std::array & pose_cov, + const double & vel, const std::array & twist_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, with velocity + constexpr int DIM_Y = 4; + + // fix yaw + double estimated_yaw = getStateElement(IDX::YAW); + double fixed_yaw = yaw; + double limiting_delta_yaw = M_PI_2; + while (std::fabs(estimated_yaw - fixed_yaw) > limiting_delta_yaw) { + if (fixed_yaw < estimated_yaw) { + fixed_yaw += 2 * limiting_delta_yaw; + } else { + fixed_yaw -= 2 * limiting_delta_yaw; + } + } + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y, yaw, vel; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + C(2, IDX::YAW) = 1.0; + C(3, IDX::VEL) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + R(3, 3) = twist_cov[utils::MSG_COV_IDX::X_X]; + + return ekf_.update(Y, C, R); +} + +bool BicycleMotionModel::limitStates() +{ + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); + if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel; + } + if (!(-motion_params_.max_slip <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= motion_params_.max_slip)) { + X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -motion_params_.max_slip : motion_params_.max_slip; + } + ekf_.init(X_t, P_t); + + return true; +} + +bool BicycleMotionModel::updateExtendedState(const double & length) +{ + lf_ = std::max(length * motion_params_.lf_ratio, motion_params_.lf_min); + lr_ = std::max(length * motion_params_.lr_ratio, motion_params_.lr_min); + return true; +} + +bool BicycleMotionModel::adjustPosition(const double & x, const double & y) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // adjust position + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + X_t(IDX::X) += x; + X_t(IDX::Y) += y; + ekf_.init(X_t, P_t); + + return true; +} + +bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) const +{ + /* Motion model: static bicycle model (constant slip angle, constant velocity) + * + * w_k = vel_k * sin(slip_k) / l_r + * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt + * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt + * yaw_{k+1} = yaw_k + w_k * dt + * vel_{k+1} = vel_k + * slip_{k+1} = slip_k + * + */ + + /* Jacobian Matrix + * + * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, + * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, + * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] + * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, + * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, + * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] + * + */ + + // Current state vector X t + Eigen::MatrixXd X_t(DIM, 1); + ekf.getX(X_t); + + const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double vel = X_t(IDX::VEL); + const double cos_slip = std::cos(X_t(IDX::SLIP)); + const double sin_slip = std::sin(X_t(IDX::SLIP)); + + double w = vel * sin_slip / lr_; + const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); + const double w_dtdt = w * dt * dt; + const double vv_dtdt__lr = vel * vel * dt * dt / lr_; + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(DIM, 1); // predicted state + X_next_t(IDX::X) = + X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt + X_next_t(IDX::Y) = + X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt + X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt + X_next_t(IDX::VEL) = X_t(IDX::VEL); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) + + // State transition matrix A + Eigen::MatrixXd A = Eigen::MatrixXd::Identity(DIM, DIM); + A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; + A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; + A(IDX::X, IDX::SLIP) = + -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; + A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; + A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; + A(IDX::Y, IDX::SLIP) = + vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; + A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; + + // Process noise covariance Q + double q_stddev_yaw_rate{0.0}; + if (vel <= 0.01) { + q_stddev_yaw_rate = motion_params_.q_stddev_yaw_rate_min; + } else { + /* uncertainty of the yaw rate is limited by the following: + * - centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v + * - or maximum slip angle slip_max : w = v*sin(slip_max)/l_r + */ + q_stddev_yaw_rate = std::min( + motion_params_.q_stddev_acc_lat / vel, + vel * std::sin(motion_params_.q_max_slip_angle) / lr_); // [rad/s] + q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, motion_params_.q_stddev_yaw_rate_max); + q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, motion_params_.q_stddev_yaw_rate_min); + } + double q_cov_slip_rate{0.0}; + if (vel <= 0.01) { + q_cov_slip_rate = motion_params_.q_cov_slip_rate_min; + } else { + /* The slip angle rate uncertainty is modeled as follows: + * d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt + * where sin(slip) = w * l_r / v + * + * d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) + * d(v)/dt and d(w)/t are considered to be uncorrelated + */ + q_cov_slip_rate = + std::pow(motion_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); + q_cov_slip_rate = std::min(q_cov_slip_rate, motion_params_.q_cov_slip_rate_max); + q_cov_slip_rate = std::max(q_cov_slip_rate, motion_params_.q_cov_slip_rate_min); + } + const double q_cov_x = std::pow(0.5 * motion_params_.q_stddev_acc_long * dt * dt, 2); + const double q_cov_y = std::pow(0.5 * motion_params_.q_stddev_acc_lat * dt * dt, 2); + const double q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); + const double q_cov_vel = std::pow(motion_params_.q_stddev_acc_long * dt, 2); + const double q_cov_slip = q_cov_slip_rate * dt * dt; + + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(DIM, DIM); + // Rotate the covariance matrix according to the vehicle yaw + // because q_cov_x and y are in the vehicle coordinate system. + Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); + Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); + Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); + Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); + Q(IDX::YAW, IDX::YAW) = q_cov_yaw; + Q(IDX::VEL, IDX::VEL) = q_cov_vel; + Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; + + // control-input model B and control-input u are not used + // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(DIM, DIM); + // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(DIM, 1); + + // predict state + return ekf.predict(X_next_t, A, Q); +} + +bool BicycleMotionModel::getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const +{ + // get predicted state + Eigen::MatrixXd X(DIM, 1); + Eigen::MatrixXd P(DIM, DIM); + if (!MotionModel::getPredictedState(time, X, P)) { + return false; + } + + // set position + pose.position.x = X(IDX::X); + pose.position.y = X(IDX::Y); + pose.position.z = 0.0; + + // set orientation + tf2::Quaternion quaternion; + quaternion.setRPY(0.0, 0.0, X(IDX::YAW)); + pose.orientation.x = quaternion.x(); + pose.orientation.y = quaternion.y(); + pose.orientation.z = quaternion.z(); + pose.orientation.w = quaternion.w(); + + // set twist + twist.linear.x = X(IDX::VEL) * std::cos(X(IDX::SLIP)); + twist.linear.y = X(IDX::VEL) * std::sin(X(IDX::SLIP)); + twist.linear.z = 0.0; + twist.angular.x = 0.0; + twist.angular.y = 0.0; + twist.angular.z = twist.linear.y / lr_; + + // set pose covariance + constexpr double zz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double rr_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double pp_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + pose_cov[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); + pose_cov[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); + pose_cov[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); + pose_cov[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); + pose_cov[utils::MSG_COV_IDX::Z_Z] = zz_cov; + pose_cov[utils::MSG_COV_IDX::ROLL_ROLL] = rr_cov; + pose_cov[utils::MSG_COV_IDX::PITCH_PITCH] = pp_cov; + + // set twist covariance + Eigen::MatrixXd cov_jacob(3, 2); + cov_jacob << std::cos(X(IDX::SLIP)), -X(IDX::VEL) * std::sin(X(IDX::SLIP)), + std::sin(X(IDX::SLIP)), X(IDX::VEL) * std::cos(X(IDX::SLIP)), std::sin(X(IDX::SLIP)) / lr_, + X(IDX::VEL) * std::cos(X(IDX::SLIP)) / lr_; + Eigen::MatrixXd cov_twist(2, 2); + cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), + P(IDX::SLIP, IDX::SLIP); + Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + twist_cov[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); + twist_cov[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); + twist_cov[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); + twist_cov[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); + twist_cov[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); + twist_cov[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); + twist_cov[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); + twist_cov[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); + twist_cov[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); + twist_cov[utils::MSG_COV_IDX::Z_Z] = vz_cov; + twist_cov[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; + twist_cov[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; + + return true; +} diff --git a/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp new file mode 100644 index 0000000000000..9e8327e381057 --- /dev/null +++ b/perception/multi_object_tracker/src/tracker/motion_model/ctrv_motion_model.cpp @@ -0,0 +1,386 @@ +// 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. +// +// +// Author: v1.0 Taekjin Lee +// + +#include "multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" + +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +// cspell: ignore CTRV +// Constant Turn Rate and constant Velocity (CTRV) motion model + +CTRVMotionModel::CTRVMotionModel() : MotionModel(), logger_(rclcpp::get_logger("CTRVMotionModel")) +{ + // Initialize motion parameters + setDefaultParams(); +} + +void CTRVMotionModel::setDefaultParams() +{ + // process noise covariance + constexpr double q_stddev_x = 0.5; // [m/s] + constexpr double q_stddev_y = 0.5; // [m/s] + constexpr double q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] + constexpr double q_stddev_vel = 9.8 * 0.3; // [m/(s*s)] + constexpr double q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] + + setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vel, q_stddev_wz); + + // set motion limitations + constexpr double max_vel = tier4_autoware_utils::kmph2mps(10); // [m/s] maximum velocity + constexpr double max_wz = 30.0; // [deg] maximum yaw rate + setMotionLimits(max_vel, max_wz); + + // set prediction parameters + constexpr double dt_max = 0.11; // [s] maximum time interval for prediction + setMaxDeltaTime(dt_max); +} + +void CTRVMotionModel::setMotionParams( + const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_yaw, + const double & q_stddev_vel, const double & q_stddev_wz) +{ + // set process noise covariance parameters + motion_params_.q_cov_x = std::pow(q_stddev_x, 2.0); + motion_params_.q_cov_y = std::pow(q_stddev_y, 2.0); + motion_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); + motion_params_.q_cov_vel = std::pow(q_stddev_vel, 2.0); + motion_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); +} + +void CTRVMotionModel::setMotionLimits(const double & max_vel, const double & max_wz) +{ + // set motion limitations + motion_params_.max_vel = max_vel; + motion_params_.max_wz = tier4_autoware_utils::deg2rad(max_wz); +} + +bool CTRVMotionModel::initialize( + const rclcpp::Time & time, const double & x, const double & y, const double & yaw, + const std::array & pose_cov, const double & vel, const double & vel_cov, + const double & wz, const double & wz_cov) +{ + // initialize state vector X + Eigen::MatrixXd X(DIM, 1); + X << x, y, yaw, vel, wz; + + // initialize covariance matrix P + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); + P(IDX::X, IDX::X) = pose_cov[utils::MSG_COV_IDX::X_X]; + P(IDX::Y, IDX::Y) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + P(IDX::YAW, IDX::YAW) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + P(IDX::VEL, IDX::VEL) = vel_cov; + P(IDX::WZ, IDX::WZ) = wz_cov; + + return MotionModel::initialize(time, X, P); +} + +bool CTRVMotionModel::updateStatePose( + const double & x, const double & y, const std::array & pose_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, without velocity + constexpr int DIM_Y = 2; + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + + return ekf_.update(Y, C, R); +} + +bool CTRVMotionModel::updateStatePoseHead( + const double & x, const double & y, const double & yaw, const std::array & pose_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, without velocity + constexpr int DIM_Y = 3; + + // fix yaw + double estimated_yaw = getStateElement(IDX::YAW); + double fixed_yaw = yaw; + double limiting_delta_yaw = M_PI_2; + while (std::fabs(estimated_yaw - fixed_yaw) > limiting_delta_yaw) { + if (fixed_yaw < estimated_yaw) { + fixed_yaw += 2 * limiting_delta_yaw; + } else { + fixed_yaw -= 2 * limiting_delta_yaw; + } + } + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y, fixed_yaw; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + C(2, IDX::YAW) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + + return ekf_.update(Y, C, R); +} + +bool CTRVMotionModel::updateStatePoseHeadVel( + const double & x, const double & y, const double & yaw, const std::array & pose_cov, + const double & vel, const std::array & twist_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, with velocity + constexpr int DIM_Y = 4; + + // fix yaw + double estimated_yaw = getStateElement(IDX::YAW); + double fixed_yaw = yaw; + double limiting_delta_yaw = M_PI_2; + while (std::fabs(estimated_yaw - fixed_yaw) > limiting_delta_yaw) { + if (fixed_yaw < estimated_yaw) { + fixed_yaw += 2 * limiting_delta_yaw; + } else { + fixed_yaw -= 2 * limiting_delta_yaw; + } + } + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y, yaw, vel; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + C(2, IDX::YAW) = 1.0; + C(3, IDX::VEL) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + R(0, 2) = pose_cov[utils::MSG_COV_IDX::X_YAW]; + R(1, 2) = pose_cov[utils::MSG_COV_IDX::Y_YAW]; + R(2, 0) = pose_cov[utils::MSG_COV_IDX::YAW_X]; + R(2, 1) = pose_cov[utils::MSG_COV_IDX::YAW_Y]; + R(2, 2) = pose_cov[utils::MSG_COV_IDX::YAW_YAW]; + R(3, 3) = twist_cov[utils::MSG_COV_IDX::X_X]; + + return ekf_.update(Y, C, R); +} + +bool CTRVMotionModel::limitStates() +{ + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); + if (!(-motion_params_.max_vel <= X_t(IDX::VEL) && X_t(IDX::VEL) <= motion_params_.max_vel)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -motion_params_.max_vel : motion_params_.max_vel; + } + if (!(-motion_params_.max_wz <= X_t(IDX::WZ) && X_t(IDX::WZ) <= motion_params_.max_wz)) { + X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -motion_params_.max_wz : motion_params_.max_wz; + } + ekf_.init(X_t, P_t); + + return true; +} + +bool CTRVMotionModel::adjustPosition(const double & x, const double & y) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // adjust position + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + X_t(IDX::X) += x; + X_t(IDX::Y) += y; + ekf_.init(X_t, P_t); + + return true; +} + +bool CTRVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) const +{ + /* Motion model: Constant Turn Rate and constant Velocity model (CTRV) + * + * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt + * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt + * yaw_{k+1} = yaw_k + (wz_k) * dt + * vx_{k+1} = vx_k + * wz_{k+1} = wz_k + * + */ + + /* Jacobian Matrix + * + * A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0] + * [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0] + * [ 0, 0, 1, 0, dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] + */ + + // Current state vector X t + Eigen::MatrixXd X_t(DIM, 1); + ekf.getX(X_t); + + const double cos_yaw = std::cos(X_t(IDX::YAW)); + const double sin_yaw = std::sin(X_t(IDX::YAW)); + const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(DIM, 1); // predicted state + X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VEL) * cos_yaw * dt; // dx = v * cos(yaw) + X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VEL) * sin_yaw * dt; // dy = v * sin(yaw) + X_next_t(IDX::YAW) = X_t(IDX::YAW) + (X_t(IDX::WZ)) * dt; // dyaw = omega + X_next_t(IDX::VEL) = X_t(IDX::VEL); + X_next_t(IDX::WZ) = X_t(IDX::WZ); + + // State transition matrix A + Eigen::MatrixXd A = Eigen::MatrixXd::Identity(DIM, DIM); + A(IDX::X, IDX::YAW) = -X_t(IDX::VEL) * sin_yaw * dt; + A(IDX::X, IDX::VEL) = cos_yaw * dt; + A(IDX::Y, IDX::YAW) = X_t(IDX::VEL) * cos_yaw * dt; + A(IDX::Y, IDX::VEL) = sin_yaw * dt; + A(IDX::YAW, IDX::WZ) = dt; + + // Process noise covariance Q + const double q_cov_x = std::pow(0.5 * motion_params_.q_cov_x * dt, 2); + const double q_cov_y = std::pow(0.5 * motion_params_.q_cov_y * dt, 2); + const double q_cov_yaw = std::pow(motion_params_.q_cov_yaw * dt, 2); + const double q_cov_vel = std::pow(motion_params_.q_cov_vel * dt, 2); + const double q_cov_wz = std::pow(motion_params_.q_cov_wz * dt, 2); + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(DIM, DIM); + // Rotate the covariance matrix according to the vehicle yaw + // because q_cov_x and y are in the vehicle coordinate system. + Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); + Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); + Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); + Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); + Q(IDX::YAW, IDX::YAW) = q_cov_yaw; + Q(IDX::VEL, IDX::VEL) = q_cov_vel; + Q(IDX::WZ, IDX::WZ) = q_cov_wz; + + // control-input model B and control-input u are not used + // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(DIM, DIM); + // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(DIM, 1); + + // predict state + return ekf.predict(X_next_t, A, Q); +} + +bool CTRVMotionModel::getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const +{ + // get predicted state + Eigen::MatrixXd X(DIM, 1); + Eigen::MatrixXd P(DIM, DIM); + if (!MotionModel::getPredictedState(time, X, P)) { + return false; + } + + // set position + pose.position.x = X(IDX::X); + pose.position.y = X(IDX::Y); + pose.position.z = 0.0; + + // set orientation + tf2::Quaternion quaternion; + quaternion.setRPY(0.0, 0.0, X(IDX::YAW)); + pose.orientation.x = quaternion.x(); + pose.orientation.y = quaternion.y(); + pose.orientation.z = quaternion.z(); + pose.orientation.w = quaternion.w(); + + // set twist + twist.linear.x = X(IDX::VEL); + twist.linear.y = 0.0; + twist.linear.z = 0.0; + twist.angular.x = 0.0; + twist.angular.y = 0.0; + twist.angular.z = X(IDX::WZ); + + // set pose covariance + constexpr double zz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double rr_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double pp_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + pose_cov[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); + pose_cov[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); + pose_cov[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); + pose_cov[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); + pose_cov[utils::MSG_COV_IDX::Z_Z] = zz_cov; + pose_cov[utils::MSG_COV_IDX::ROLL_ROLL] = rr_cov; + pose_cov[utils::MSG_COV_IDX::PITCH_PITCH] = pp_cov; + + // set twist covariance + constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + twist_cov[utils::MSG_COV_IDX::X_X] = P(IDX::VEL, IDX::VEL); + twist_cov[utils::MSG_COV_IDX::X_Y] = 0.0; + twist_cov[utils::MSG_COV_IDX::X_YAW] = P(IDX::VEL, IDX::WZ); + twist_cov[utils::MSG_COV_IDX::Y_X] = 0.0; + twist_cov[utils::MSG_COV_IDX::Y_Y] = vy_cov; + twist_cov[utils::MSG_COV_IDX::Y_YAW] = 0.0; + twist_cov[utils::MSG_COV_IDX::YAW_X] = P(IDX::WZ, IDX::VEL); + twist_cov[utils::MSG_COV_IDX::YAW_Y] = 0.0; + twist_cov[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ); + twist_cov[utils::MSG_COV_IDX::Z_Z] = vz_cov; + twist_cov[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; + twist_cov[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; + + return true; +} diff --git a/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp new file mode 100644 index 0000000000000..a9ad03e7875c5 --- /dev/null +++ b/perception/multi_object_tracker/src/tracker/motion_model/cv_motion_model.cpp @@ -0,0 +1,307 @@ +// 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. +// +// +// Author: v1.0 Taekjin Lee +// + +#include "multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" + +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include + +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +// cspell: ignore CV +// Constant Velocity (CV) motion model + +CVMotionModel::CVMotionModel() : MotionModel(), logger_(rclcpp::get_logger("CVMotionModel")) +{ + // Initialize motion parameters + setDefaultParams(); +} + +void CVMotionModel::setDefaultParams() +{ + // process noise covariance + constexpr double q_stddev_x = 0.5; // [m/s] standard deviation of x + constexpr double q_stddev_y = 0.5; // [m/s] standard deviation of y + constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] standard deviation of vx + constexpr double q_stddev_vy = 9.8 * 0.3; // [m/(s*s)] standard deviation of vy + setMotionParams(q_stddev_x, q_stddev_y, q_stddev_vx, q_stddev_vy); + + // set motion limitations + constexpr double max_vx = tier4_autoware_utils::kmph2mps(60); // [m/s] maximum x velocity + constexpr double max_vy = tier4_autoware_utils::kmph2mps(60); // [m/s] maximum y velocity + setMotionLimits(max_vx, max_vy); + + // set prediction parameters + constexpr double dt_max = 0.11; // [s] maximum time interval for prediction + setMaxDeltaTime(dt_max); +} + +void CVMotionModel::setMotionParams( + const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_vx, + const double & q_stddev_vy) +{ + // set process noise covariance parameters + motion_params_.q_cov_x = std::pow(q_stddev_x, 2.0); + motion_params_.q_cov_y = std::pow(q_stddev_y, 2.0); + motion_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); + motion_params_.q_cov_vy = std::pow(q_stddev_vy, 2.0); +} + +void CVMotionModel::setMotionLimits(const double & max_vx, const double & max_vy) +{ + // set motion limitations + motion_params_.max_vx = max_vx; + motion_params_.max_vy = max_vy; +} + +bool CVMotionModel::initialize( + const rclcpp::Time & time, const double & x, const double & y, + const std::array & pose_cov, const double & vx, const double & vy, + const std::array & twist_cov) +{ + // initialize state vector X + Eigen::MatrixXd X(DIM, 1); + X << x, y, vx, vy; + + // initialize covariance matrix P + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(DIM, DIM); + P(IDX::X, IDX::X) = pose_cov[utils::MSG_COV_IDX::X_X]; + P(IDX::Y, IDX::Y) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + P(IDX::VX, IDX::VX) = twist_cov[utils::MSG_COV_IDX::X_X]; + P(IDX::VY, IDX::VY) = twist_cov[utils::MSG_COV_IDX::Y_Y]; + + return MotionModel::initialize(time, X, P); +} + +bool CVMotionModel::updateStatePose( + const double & x, const double & y, const std::array & pose_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state, without velocity + constexpr int DIM_Y = 2; + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + + return ekf_.update(Y, C, R); +} + +bool CVMotionModel::updateStatePoseVel( + const double & x, const double & y, const std::array & pose_cov, const double & vx, + const double & vy, const std::array & twist_cov) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // update state with velocity + constexpr int DIM_Y = 4; + + // update state + Eigen::MatrixXd Y(DIM_Y, 1); + Y << x, y, vx, vy; + + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(DIM_Y, DIM); + C(0, IDX::X) = 1.0; + C(1, IDX::Y) = 1.0; + C(2, IDX::VX) = 1.0; + C(3, IDX::VY) = 1.0; + + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(DIM_Y, DIM_Y); + R(0, 0) = pose_cov[utils::MSG_COV_IDX::X_X]; + R(0, 1) = pose_cov[utils::MSG_COV_IDX::X_Y]; + R(1, 0) = pose_cov[utils::MSG_COV_IDX::Y_X]; + R(1, 1) = pose_cov[utils::MSG_COV_IDX::Y_Y]; + R(2, 2) = twist_cov[utils::MSG_COV_IDX::X_X]; + R(2, 3) = twist_cov[utils::MSG_COV_IDX::X_Y]; + R(3, 2) = twist_cov[utils::MSG_COV_IDX::Y_X]; + R(3, 3) = twist_cov[utils::MSG_COV_IDX::Y_Y]; + + return ekf_.update(Y, C, R); +} + +bool CVMotionModel::limitStates() +{ + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + if (!(-motion_params_.max_vx <= X_t(IDX::VX) && X_t(IDX::VX) <= motion_params_.max_vx)) { + X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -motion_params_.max_vx : motion_params_.max_vx; + } + if (!(-motion_params_.max_vy <= X_t(IDX::VY) && X_t(IDX::VY) <= motion_params_.max_vy)) { + X_t(IDX::VY) = X_t(IDX::VY) < 0 ? -motion_params_.max_vy : motion_params_.max_vy; + } + ekf_.init(X_t, P_t); + + return true; +} + +bool CVMotionModel::adjustPosition(const double & x, const double & y) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + // adjust position + Eigen::MatrixXd X_t(DIM, 1); + Eigen::MatrixXd P_t(DIM, DIM); + ekf_.getX(X_t); + ekf_.getP(P_t); + X_t(IDX::X) += x; + X_t(IDX::Y) += y; + ekf_.init(X_t, P_t); + + return true; +} + +bool CVMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) const +{ + /* Motion model: Constant velocity model + * + * x_{k+1} = x_k + vx_k * dt + * y_{k+1} = y_k + vx_k * dt + * vx_{k+1} = vx_k + * vy_{k+1} = vy_k + * + */ + + /* Jacobian Matrix + * + * A = [ 1, 0, dt, 0] + * [ 0, 1, 0, dt] + * [ 0, 0, 1, 0] + * [ 0, 0, 0, 1] + */ + + // Current state vector X t + Eigen::MatrixXd X_t(DIM, 1); + ekf.getX(X_t); + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(DIM, 1); // predicted state + X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * dt; + X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VY) * dt; + X_next_t(IDX::VX) = X_t(IDX::VX); + X_next_t(IDX::VY) = X_t(IDX::VY); + + // State transition matrix A + Eigen::MatrixXd A = Eigen::MatrixXd::Identity(DIM, DIM); + A(IDX::X, IDX::VX) = dt; + A(IDX::Y, IDX::VY) = dt; + + // Process noise covariance Q + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(DIM, DIM); + Q(IDX::X, IDX::X) = motion_params_.q_cov_x * dt * dt; + Q(IDX::X, IDX::Y) = 0.0; + Q(IDX::Y, IDX::Y) = motion_params_.q_cov_y * dt * dt; + Q(IDX::Y, IDX::X) = 0.0; + Q(IDX::VX, IDX::VX) = motion_params_.q_cov_vx * dt * dt; + Q(IDX::VY, IDX::VY) = motion_params_.q_cov_vy * dt * dt; + + // control-input model B and control-input u are not used + // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(DIM, DIM); + // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(DIM, 1); + + // predict state + return ekf.predict(X_next_t, A, Q); +} + +bool CVMotionModel::getPredictedState( + const rclcpp::Time & time, geometry_msgs::msg::Pose & pose, std::array & pose_cov, + geometry_msgs::msg::Twist & twist, std::array & twist_cov) const +{ + // get predicted state + Eigen::MatrixXd X(DIM, 1); + Eigen::MatrixXd P(DIM, DIM); + if (!MotionModel::getPredictedState(time, X, P)) { + return false; + } + + // get yaw from pose + const double yaw = tf2::getYaw(pose.orientation); + + // set position + pose.position.x = X(IDX::X); + pose.position.y = X(IDX::Y); + pose.position.z = 0.0; + + // set twist + twist.linear.x = X(IDX::VX) * std::cos(-yaw) - X(IDX::VY) * std::sin(-yaw); + twist.linear.y = X(IDX::VX) * std::sin(-yaw) + X(IDX::VY) * std::cos(-yaw); + twist.linear.z = 0.0; + twist.angular.x = 0.0; + twist.angular.y = 0.0; + twist.angular.z = 0.0; + + // set pose covariance + constexpr double zz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double rr_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double pp_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double yaw_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + pose_cov[utils::MSG_COV_IDX::X_X] = P(IDX::X, IDX::X); + pose_cov[utils::MSG_COV_IDX::X_Y] = P(IDX::X, IDX::Y); + pose_cov[utils::MSG_COV_IDX::Y_X] = P(IDX::Y, IDX::X); + pose_cov[utils::MSG_COV_IDX::Y_Y] = P(IDX::Y, IDX::Y); + pose_cov[utils::MSG_COV_IDX::Z_Z] = zz_cov; + pose_cov[utils::MSG_COV_IDX::ROLL_ROLL] = rr_cov; + pose_cov[utils::MSG_COV_IDX::PITCH_PITCH] = pp_cov; + pose_cov[utils::MSG_COV_IDX::YAW_YAW] = yaw_cov; + + // set twist covariance + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + constexpr double wz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + // rotate covariance matrix + Eigen::MatrixXd twist_cov_rotate(2, 2); + twist_cov_rotate(0, 0) = P(IDX::VX, IDX::VX); + twist_cov_rotate(0, 1) = P(IDX::VX, IDX::VY); + twist_cov_rotate(1, 0) = P(IDX::VY, IDX::VX); + twist_cov_rotate(1, 1) = P(IDX::VY, IDX::VY); + Eigen::MatrixXd R_yaw = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); + Eigen::MatrixXd twist_cov_rotated = R_yaw * twist_cov_rotate * R_yaw.transpose(); + twist_cov[utils::MSG_COV_IDX::X_X] = twist_cov_rotated(0, 0); + twist_cov[utils::MSG_COV_IDX::X_Y] = twist_cov_rotated(0, 1); + twist_cov[utils::MSG_COV_IDX::Y_X] = twist_cov_rotated(1, 0); + twist_cov[utils::MSG_COV_IDX::Y_Y] = twist_cov_rotated(1, 1); + twist_cov[utils::MSG_COV_IDX::Z_Z] = vz_cov; + twist_cov[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; + twist_cov[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; + twist_cov[utils::MSG_COV_IDX::YAW_YAW] = wz_cov; + + return true; +} diff --git a/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp b/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp new file mode 100644 index 0000000000000..4d51021de634b --- /dev/null +++ b/perception/multi_object_tracker/src/tracker/motion_model/motion_model_base.cpp @@ -0,0 +1,92 @@ +// 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. +// +// +// Author: v1.0 Taekjin Lee +// + +#include "multi_object_tracker/tracker/motion_model/motion_model_base.hpp" + +MotionModel::MotionModel() : last_update_time_(rclcpp::Time(0, 0)) +{ +} + +bool MotionModel::initialize( + const rclcpp::Time & time, const Eigen::MatrixXd & X, const Eigen::MatrixXd & P) +{ + // initialize Kalman filter + if (!ekf_.init(X, P)) return false; + + // set last_update_time_ + last_update_time_ = time; + + // set initialized flag + is_initialized_ = true; + + return true; +} + +bool MotionModel::predictState(const rclcpp::Time & time) +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + const double dt = getDeltaTime(time); + if (dt < 0.0) { + return false; + } + if (dt < 1e-6 /*1usec*/) { + return true; + } + + // multi-step prediction + // if dt is too large, shorten dt and repeat prediction + const uint32_t repeat = std::floor(dt / dt_max_) + 1; + const double dt_ = dt / repeat; + for (uint32_t i = 0; i < repeat; ++i) { + if (!predictStateStep(dt_, ekf_)) return false; + last_update_time_ += rclcpp::Duration::from_seconds(dt_); + } + // reset the last_update_time_ to the prediction time + last_update_time_ = time; + return true; +} + +bool MotionModel::getPredictedState( + const rclcpp::Time & time, Eigen::MatrixXd & X, Eigen::MatrixXd & P) const +{ + // check if the state is initialized + if (!checkInitialized()) return false; + + const double dt = getDeltaTime(time); + if (dt < 1e-6 /*1usec*/) { + // no prediction, return the current state + ekf_.getX(X); + ekf_.getP(P); + return true; + } + + // copy the predicted state and covariance + KalmanFilter tmp_ekf_for_no_update = ekf_; + // multi-step prediction + // if dt is too large, shorten dt and repeat prediction + const uint32_t repeat = std::floor(dt / dt_max_) + 1; + const double dt_ = dt / repeat; + for (uint32_t i = 0; i < repeat; ++i) { + if (!predictStateStep(dt_, tmp_ekf_for_no_update)) return false; + } + tmp_ekf_for_no_update.getX(X); + tmp_ekf_for_no_update.getP(P); + return true; +} diff --git a/perception/object_merger/include/object_merger/node.hpp b/perception/object_merger/include/object_merger/node.hpp index 8341ce490ca72..1194de2558f58 100644 --- a/perception/object_merger/include/object_merger/node.hpp +++ b/perception/object_merger/include/object_merger/node.hpp @@ -18,6 +18,9 @@ #include "object_merger/data_association/data_association.hpp" #include +#include +#include +#include #include @@ -81,6 +84,12 @@ class ObjectAssociationMergerNode : public rclcpp::Node std::map generalized_iou_threshold; std::map distance_threshold_map; } overlapped_judge_param_; + + // debug publisher + std::unique_ptr processing_time_publisher_; + std::unique_ptr> stop_watch_ptr_; + + std::unique_ptr published_time_publisher_; }; } // namespace object_association diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index 1ffc2791e8f4e..ec5ad62b52a29 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -118,6 +118,14 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio merged_object_pub_ = create_publisher( "output/object", rclcpp::QoS{1}); + + // Debug publisher + processing_time_publisher_ = + std::make_unique(this, "object_association_merger"); + stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + published_time_publisher_ = std::make_unique(this); } void ObjectAssociationMergerNode::objectsCallback( @@ -128,6 +136,7 @@ void ObjectAssociationMergerNode::objectsCallback( if (merged_object_pub_->get_subscription_count() < 1) { return; } + stop_watch_ptr_->toc("processing_time", true); /* transform to base_link coordinate */ autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects0, transformed_objects1; @@ -215,6 +224,12 @@ void ObjectAssociationMergerNode::objectsCallback( // publish output msg merged_object_pub_->publish(output_msg); + published_time_publisher_->publish_if_subscribed(merged_object_pub_, output_msg.header.stamp); + // publish processing time + processing_time_publisher_->publish( + "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); + processing_time_publisher_->publish( + "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } } // namespace object_association diff --git a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp index 953307e9d5380..1d7d01bfbab55 100644 --- a/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp +++ b/perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -120,6 +121,7 @@ class OccupancyGridMapOutlierFilterComponent : public rclcpp::Node std::shared_ptr debugger_ptr_; std::unique_ptr> stop_watch_ptr_; std::unique_ptr debug_publisher_; + std::unique_ptr published_time_publisher_; // ROS Parameters std::string map_frame_; diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 691c13a6d4701..65eb336269fe4 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -234,24 +234,48 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( if (enable_debugger) { debugger_ptr_ = std::make_shared(*this); } + published_time_publisher_ = std::make_unique(this); } void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack( const PointCloud2::ConstSharedPtr & input_pc, PointCloud2 & front_pc, PointCloud2 & behind_pc) { - PclPointCloud tmp_behind_pc; - PclPointCloud tmp_front_pc; - for (sensor_msgs::PointCloud2ConstIterator x(*input_pc, "x"), y(*input_pc, "y"), - z(*input_pc, "z"); - x != x.end(); ++x, ++y, ++z) { + size_t front_count = 0; + size_t behind_count = 0; + + for (sensor_msgs::PointCloud2ConstIterator x(*input_pc, "x"); x != x.end(); ++x) { if (*x < 0.0) { - tmp_behind_pc.push_back(pcl::PointXYZ(*x, *y, *z)); + behind_count++; } else { - tmp_front_pc.push_back(pcl::PointXYZ(*x, *y, *z)); + front_count++; } } - pcl::toROSMsg(tmp_front_pc, front_pc); - pcl::toROSMsg(tmp_behind_pc, behind_pc); + + sensor_msgs::PointCloud2Modifier front_pc_modifier(front_pc); + sensor_msgs::PointCloud2Modifier behind_pc_modifier(behind_pc); + front_pc_modifier.setPointCloud2FieldsByString(1, "xyz"); + behind_pc_modifier.setPointCloud2FieldsByString(1, "xyz"); + front_pc_modifier.resize(front_count); + behind_pc_modifier.resize(behind_count); + + sensor_msgs::PointCloud2Iterator fr_iter(front_pc, "x"); + sensor_msgs::PointCloud2Iterator be_iter(behind_pc, "x"); + + for (sensor_msgs::PointCloud2ConstIterator in_iter(*input_pc, "x"); + in_iter != in_iter.end(); ++in_iter) { + if (*in_iter < 0.0) { + *be_iter = in_iter[0]; + be_iter[1] = in_iter[1]; + be_iter[2] = in_iter[2]; + ++be_iter; + } else { + *fr_iter = in_iter[0]; + fr_iter[1] = in_iter[1]; + fr_iter[2] = in_iter[2]; + ++fr_iter; + } + } + front_pc.header = input_pc->header; behind_pc.header = input_pc->header; } @@ -305,6 +329,8 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( return; } pointcloud_pub_->publish(std::move(base_link_frame_filtered_pc_ptr)); + published_time_publisher_->publish_if_subscribed( + pointcloud_pub_, ogm_frame_filtered_pc.header.stamp); } if (debugger_ptr_) { debugger_ptr_->publishHighConfidence(high_confidence_pc, ogm_frame_pc.header); diff --git a/perception/probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml b/perception/probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml new file mode 100644 index 0000000000000..f9a1c4f930001 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml @@ -0,0 +1,53 @@ +# sample grid map fusion parameters for sample sensor kit +/**: + ros__parameters: + # shared parameters + shared_config: + map_frame: "map" + base_link_frame: "base_link" + # center of the grid map + gridmap_origin_frame: "base_link" + + map_resolution: 0.5 # [m] + map_length_x: 150.0 # [m] + map_length_y: 150.0 # [m] + + # each grid map parameters + ogm_creation_config: + height_filter: + use_height_filter: true + min_height: -1.0 + max_height: 2.0 + enable_single_frame_mode: true + # use sensor pointcloud to filter obstacle pointcloud + filter_obstacle_pointcloud_by_raw_pointcloud: true + + grid_map_type: "OccupancyGridMapFixedBlindSpot" + OccupancyGridMapFixedBlindSpot: + distance_margin: 1.0 + OccupancyGridMapProjectiveBlindSpot: + projection_dz_threshold: 0.01 # [m] for avoiding null division + obstacle_separation_threshold: 1.0 # [m] fill the interval between obstacles with unknown for this length + pub_debug_grid: false + + # parameter settings for ogm fusion + fusion_config: + # following parameters are shared: map_frame, base_link_frame, gridmap_origin_frame, map_resolution, map_length + # Setting1: tune ogm creation parameters + raw_pointcloud_topics: # put each sensor's pointcloud topic + - "/sensing/lidar/top/pointcloud" + - "/sensing/lidar/left/pointcloud" + - "/sensing/lidar/right/pointcloud" + fusion_input_ogm_topics: + - "/perception/occupancy_grid_map/top_lidar/map" + - "/perception/occupancy_grid_map/left_lidar/map" + - "/perception/occupancy_grid_map/right_lidar/map" + # reliability of each sensor (0.0 ~ 1.0) only work with "log-odds" and "dempster-shafer" + input_ogm_reliabilities: + - 1.0 + - 0.6 + - 0.6 + + # Setting2: tune ogm fusion parameters + ## choose fusion method from ["overwrite", "log-odds", "dempster-shafer"] + fusion_method: "overwrite" diff --git a/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py new file mode 100644 index 0000000000000..29f8ed5985d48 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/launch/multi_lidar_pointcloud_based_occupancy_grid_map.launch.py @@ -0,0 +1,211 @@ +# Copyright 2021 Tier IV, Inc. All rights reserved. +# +# 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. + + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode +import yaml + +# In this file, we use "ogm" as a meaning of occupancy grid map + + +# overwrite parameter +def overwrite_config(param_dict, launch_config_name, node_params_name, context): + if LaunchConfiguration(launch_config_name).perform(context) != "": + param_dict[node_params_name] = LaunchConfiguration(launch_config_name).perform(context) + + +# load parameter files +def load_config_file(context, configuration_name: str): + config_file = LaunchConfiguration(configuration_name).perform(context) + with open(config_file, "r") as f: + config_param_dict = yaml.safe_load(f)["/**"]["ros__parameters"] + return config_param_dict + + +# check if the length of the parameters are the same +def fusion_config_sanity_check(fusion_config: dict): + listed_param_names = [ + "raw_pointcloud_topics", + "fusion_input_ogm_topics", + # "each_ogm_sensor_frames", + "input_ogm_reliabilities", + ] + param_length_list = [] + + for param_name in listed_param_names: + # parameters is not in the config file dict + if param_name not in fusion_config: + raise Exception("Error: " + param_name + " is not in the config file") + # get len of the parameter + param_length_list.append(len(fusion_config[param_name])) + + # check if the length of the parameters are the same + if not all(x == param_length_list[0] for x in param_length_list): + raise Exception("Error: the length of the parameters are not the same") + + +def get_fusion_config(total_config: dict) -> dict: + fusion_config = total_config["fusion_config"] + shared_config = total_config["shared_config"] + # merge shared config and fusion config + fusion_config.update(shared_config) + # overwrite ogm size settings + fusion_config["fusion_map_length_x"] = shared_config["map_length_x"] + fusion_config["fusion_map_length_y"] = shared_config["map_length_y"] + fusion_config["fusion_map_resolution"] = shared_config["map_resolution"] + return fusion_config + + +# extract ogm creation config from fusion config +def get_ogm_creation_config(total_config: dict, list_iter: int) -> dict: + ogm_creation_config = total_config["ogm_creation_config"] + shared_config = total_config["shared_config"] + # fusion_config_ = total_config["fusion_config"] + # merge shared config and ogm creation config + ogm_creation_config.update(shared_config) + # overwrite scan_origin_frame with sensor frame settings + # ogm_creation_config["scan_origin_frame"] = fusion_config_["each_ogm_sensor_frames"][list_iter] + # use fusion_map_length to set map_length + ogm_creation_config["map_length"] = max( + shared_config["map_length_x"], shared_config["map_length_y"] + ) + return ogm_creation_config + + +def launch_setup(context, *args, **kwargs): + """Launch fusion based occupancy grid map creation nodes. + + 1. describe occupancy grid map generation nodes for each sensor input + 2. describe occupancy grid map fusion node + 3. launch setting + """ + # 1. launch occupancy grid map generation nodes for each sensor input + + # load fusion config parameter + total_config = load_config_file(context, "multi_lidar_fusion_config_file") + fusion_config = get_fusion_config(total_config) + fusion_config_sanity_check(fusion_config) + updater_config = load_config_file(context, "updater_param_file") + + # pointcloud based occupancy grid map nodes + gridmap_generation_composable_nodes = [] + + number_of_nodes = len(fusion_config["raw_pointcloud_topics"]) + print(number_of_nodes) + + for i in range(number_of_nodes): + # load parameter file + ogm_creation_config = get_ogm_creation_config(total_config, i) + ogm_creation_config["updater_type"] = LaunchConfiguration("updater_type").perform(context) + ogm_creation_config.update(updater_config) + + # generate composable node + node = ComposableNode( + package="probabilistic_occupancy_grid_map", + plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", + name="occupancy_grid_map_node_in_" + str(i), + remappings=[ + ("~/input/obstacle_pointcloud", LaunchConfiguration("input/obstacle_pointcloud")), + ("~/input/raw_pointcloud", fusion_config["raw_pointcloud_topics"][i]), + ("~/output/occupancy_grid_map", fusion_config["fusion_input_ogm_topics"][i]), + ], + parameters=[ogm_creation_config], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + gridmap_generation_composable_nodes.append(node) + + # 2. launch occupancy grid map fusion node + gridmap_fusion_node = [ + ComposableNode( + package="probabilistic_occupancy_grid_map", + plugin="synchronized_grid_map_fusion::GridMapFusionNode", + name="occupancy_grid_map_fusion_node", + remappings=[ + ("~/output/occupancy_grid_map", LaunchConfiguration("output")), + ], + parameters=[fusion_config], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ), + ] + + # 3. launch setting + occupancy_grid_map_container = ComposableNodeContainer( + name=LaunchConfiguration("container_name"), + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=gridmap_generation_composable_nodes + gridmap_fusion_node, + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), + output="screen", + ) + + load_composable_nodes = LoadComposableNodes( + composable_node_descriptions=gridmap_generation_composable_nodes + gridmap_fusion_node, + target_container=LaunchConfiguration("container_name"), + condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), + ) + + return [occupancy_grid_map_container, load_composable_nodes] + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + return LaunchDescription( + [ + add_launch_arg("use_multithread", "false"), + add_launch_arg("use_intra_process", "true"), + add_launch_arg("use_pointcloud_container", "false"), + add_launch_arg("container_name", "occupancy_grid_map_container"), + add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"), + add_launch_arg("output", "occupancy_grid"), + add_launch_arg( + "multi_lidar_fusion_config_file", + get_package_share_directory("probabilistic_occupancy_grid_map") + + "/config/multi_lidar_pointcloud_based_occupancy_grid_map.param.yaml", + ), + add_launch_arg("updater_type", "binary_bayes_filter"), + add_launch_arg( + "updater_param_file", + get_package_share_directory("probabilistic_occupancy_grid_map") + + "/config/binary_bayes_filter_updater.param.yaml", + ), + set_container_executable, + set_container_mt_executable, + ] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp index 28fe4adafb482..f191a23eb8e65 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -311,10 +311,16 @@ void GridMapFusionNode::publish() if (debug_publisher_ptr_ && stop_watch_ptr_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds((this->get_clock()->now() - latest_stamp).nanoseconds())) + .count(); debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); + debug_publisher_ptr_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index a8cdff5b27907..96afb0f2e852c 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -58,7 +58,7 @@ PointcloudBasedOccupancyGridMapNode::PointcloudBasedOccupancyGridMapNode( map_frame_ = this->declare_parameter("map_frame"); base_link_frame_ = this->declare_parameter("base_link_frame"); gridmap_origin_frame_ = this->declare_parameter("gridmap_origin_frame"); - scan_origin_frame_ = this->declare_parameter("scan_origin_frame"); + scan_origin_frame_ = this->declare_parameter("scan_origin_frame", ""); use_height_filter_ = this->declare_parameter("height_filter.use_height_filter"); min_height_ = this->declare_parameter("height_filter.min_height"); max_height_ = this->declare_parameter("height_filter.max_height"); @@ -136,6 +136,11 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( if (stop_watch_ptr_) { stop_watch_ptr_->toc("processing_time", true); } + // if scan_origin_frame_ is "", replace it with input_raw_msg->header.frame_id + if (scan_origin_frame_.empty()) { + scan_origin_frame_ = input_raw_msg->header.frame_id; + } + // Apply height filter PointCloud2 cropped_obstacle_pc{}; PointCloud2 cropped_raw_pc{}; diff --git a/perception/radar_tracks_msgs_converter/README.md b/perception/radar_tracks_msgs_converter/README.md index b55755fb96f92..e976e342e3d9f 100644 --- a/perception/radar_tracks_msgs_converter/README.md +++ b/perception/radar_tracks_msgs_converter/README.md @@ -52,6 +52,12 @@ Autoware objects label is defined in [ObjectClassification.idl](https://gitlab.c ### Parameters +#### Parameter Summary + +{{ json_to_markdown("perception/radar_tracks_msgs_converter/schema/radar_tracks_msgs_converter.schema.json") }} + +#### Parameter Description + - `update_rate_hz` (double) [hz] - Default parameter is 20.0 diff --git a/perception/radar_tracks_msgs_converter/schema/radar_tracks_msgs_converter.schema.json b/perception/radar_tracks_msgs_converter/schema/radar_tracks_msgs_converter.schema.json new file mode 100644 index 0000000000000..396dbcd413ee8 --- /dev/null +++ b/perception/radar_tracks_msgs_converter/schema/radar_tracks_msgs_converter.schema.json @@ -0,0 +1,57 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Radar Tracks Msgs Converter node", + "type": "object", + "definitions": { + "radar_tracks_msgs_converter": { + "type": "object", + "properties": { + "update_rate_hz": { + "type": "number", + "description": "The update rate [hz] of the output topic", + "default": "20.0", + "minimum": 0.0 + }, + "new_frame_id": { + "type": "string", + "description": "The header frame_id of the output topic", + "default": "base_link" + }, + "use_twist_compensation": { + "type": "boolean", + "description": "Flag to enable the linear compensation of ego vehicle's twist", + "default": "false" + }, + "use_twist_yaw_compensation": { + "type": "boolean", + "description": "Flag to enable the compensation of yaw rotation of ego vehicle's twist", + "default": "false" + }, + "static_object_speed_threshold": { + "type": "number", + "description": "Threshold to treat detected objects as static objects", + "default": "1.0" + } + }, + "required": [ + "update_rate_hz", + "new_frame_id", + "use_twist_compensation", + "use_twist_yaw_compensation", + "static_object_speed_threshold" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/radar_tracks_msgs_converter" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/shape_estimation/config/shape_estimation.param.yaml b/perception/shape_estimation/config/shape_estimation.param.yaml index 253516fffe0d4..e277d99d70edd 100644 --- a/perception/shape_estimation/config/shape_estimation.param.yaml +++ b/perception/shape_estimation/config/shape_estimation.param.yaml @@ -5,3 +5,4 @@ use_vehicle_reference_yaw: false use_vehicle_reference_shape_size: false use_boost_bbox_optimizer: false + fix_filtered_objects_label_to_unknown: true diff --git a/perception/shape_estimation/lib/shape_estimator.cpp b/perception/shape_estimation/lib/shape_estimator.cpp index e89eaac0a6db0..e0be98edf926a 100644 --- a/perception/shape_estimation/lib/shape_estimator.cpp +++ b/perception/shape_estimation/lib/shape_estimator.cpp @@ -39,14 +39,15 @@ bool ShapeEstimator::estimateShapeAndPose( autoware_auto_perception_msgs::msg::Shape shape; geometry_msgs::msg::Pose pose; // estimate shape + bool reverse_to_unknown = false; if (!estimateOriginalShapeAndPose(label, cluster, ref_yaw_info, shape, pose)) { - return false; + reverse_to_unknown = true; } // rule based filter if (use_filter_) { if (!applyFilter(label, shape, pose)) { - return false; + reverse_to_unknown = true; } } @@ -54,10 +55,15 @@ bool ShapeEstimator::estimateShapeAndPose( if (use_corrector_) { bool use_reference_yaw = ref_yaw_info ? true : false; if (!applyCorrector(label, use_reference_yaw, ref_shape_size_info, shape, pose)) { - return false; + reverse_to_unknown = true; } } - + if (reverse_to_unknown) { + estimateOriginalShapeAndPose(Label::UNKNOWN, cluster, ref_yaw_info, shape, pose); + shape_output = shape; + pose_output = pose; + return false; + } shape_output = shape; pose_output = pose; return true; diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index 624ca604d8fbf..9d7e8b4d4729d 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -47,13 +47,23 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option use_vehicle_reference_yaw_ = declare_parameter("use_vehicle_reference_yaw"); use_vehicle_reference_shape_size_ = declare_parameter("use_vehicle_reference_shape_size"); bool use_boost_bbox_optimizer = declare_parameter("use_boost_bbox_optimizer"); + fix_filtered_objects_label_to_unknown_ = + declare_parameter("fix_filtered_objects_label_to_unknown"); RCLCPP_INFO(this->get_logger(), "using boost shape estimation : %d", use_boost_bbox_optimizer); estimator_ = std::make_unique(use_corrector, use_filter, use_boost_bbox_optimizer); + + processing_time_publisher_ = + std::make_unique(this, "shape_estimation"); + stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + published_time_publisher_ = std::make_unique(this); } void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstSharedPtr input_msg) { + stop_watch_ptr_->toc("processing_time", true); // Guard if (pub_->get_subscription_count() < 1) { return; @@ -96,18 +106,26 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared const bool estimated_success = estimator_->estimateShapeAndPose( label, *cluster, ref_yaw_info, ref_shape_size_info, shape, pose); - // If the shape estimation fails, ignore it. - if (!estimated_success) { + // If the shape estimation fails, change to Unknown object. + if (!fix_filtered_objects_label_to_unknown_ && !estimated_success) { continue; } - output_msg.feature_objects.push_back(feature_object); + if (!estimated_success) { + output_msg.feature_objects.back().object.classification.front().label = Label::UNKNOWN; + } + output_msg.feature_objects.back().object.shape = shape; output_msg.feature_objects.back().object.kinematics.pose_with_covariance.pose = pose; } // Publish pub_->publish(output_msg); + published_time_publisher_->publish_if_subscribed(pub_, output_msg.header.stamp); + processing_time_publisher_->publish( + "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); + processing_time_publisher_->publish( + "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } #include diff --git a/perception/shape_estimation/src/node.hpp b/perception/shape_estimation/src/node.hpp index 95ee4afe8d9e8..012261e79e8dd 100644 --- a/perception/shape_estimation/src/node.hpp +++ b/perception/shape_estimation/src/node.hpp @@ -18,6 +18,9 @@ #include "shape_estimation/shape_estimator.hpp" #include +#include +#include +#include #include #include @@ -32,12 +35,18 @@ class ShapeEstimationNode : public rclcpp::Node // ros rclcpp::Publisher::SharedPtr pub_; rclcpp::Subscription::SharedPtr sub_; + std::unique_ptr published_time_publisher_; + + // debug publisher + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; void callback(const DetectedObjectsWithFeature::ConstSharedPtr input_msg); std::unique_ptr estimator_; bool use_vehicle_reference_yaw_; bool use_vehicle_reference_shape_size_; + bool fix_filtered_objects_label_to_unknown_; public: explicit ShapeEstimationNode(const rclcpp::NodeOptions & node_options); diff --git a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp index 0509fb2a07dc5..357ed9c986301 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp +++ b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp @@ -20,6 +20,9 @@ #include "tracking_object_merger/utils/utils.hpp" #include +#include +#include +#include #include #include @@ -87,6 +90,8 @@ class DecorativeTrackerMergerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_object_pub_; bool publish_interpolated_sub_objects_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr processing_time_publisher_; /* handle objects */ std::unordered_map> @@ -106,6 +111,8 @@ class DecorativeTrackerMergerNode : public rclcpp::Node // tracker default settings TrackerStateParameter tracker_state_parameter_; + std::unique_ptr published_time_publisher_; + // merge policy (currently not used) struct { diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp index b22b8d5fa2948..2273ad6504e2a 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp +++ b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp @@ -151,6 +151,14 @@ DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptio set3dDataAssociation("lidar-radar", data_association_map_); // radar-radar association matrix set3dDataAssociation("radar-radar", data_association_map_); + + // debug publisher + processing_time_publisher_ = + std::make_unique(this, "decorative_object_merger_node"); + stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + published_time_publisher_ = std::make_unique(this); } void DecorativeTrackerMergerNode::set3dDataAssociation( @@ -182,6 +190,7 @@ void DecorativeTrackerMergerNode::set3dDataAssociation( void DecorativeTrackerMergerNode::mainObjectsCallback( const TrackedObjects::ConstSharedPtr & main_objects) { + stop_watch_ptr_->toc("processing_time", true); // try to merge sub object if (!sub_objects_buffer_.empty()) { // get interpolated sub objects @@ -212,8 +221,14 @@ void DecorativeTrackerMergerNode::mainObjectsCallback( // try to merge main object this->decorativeMerger(main_sensor_type_, main_objects); - - merged_object_pub_->publish(getTrackedObjects(main_objects->header)); + const auto & tracked_objects = getTrackedObjects(main_objects->header); + merged_object_pub_->publish(tracked_objects); + published_time_publisher_->publish_if_subscribed( + merged_object_pub_, tracked_objects.header.stamp); + processing_time_publisher_->publish( + "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); + processing_time_publisher_->publish( + "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } /** diff --git a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml index 3e8907cdccdf6..ad5fe0b123f1d 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml +++ b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml @@ -12,64 +12,80 @@ moving_time_threshold: 1.0 # [s] max_expand_ratio: 0.0 # [-] envelope_buffer_margin: 0.3 # [m] - avoid_margin_lateral: 1.0 # [m] - safety_buffer_lateral: 0.7 # [m] + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] truck: execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] bus: execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] trailer: execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] unknown: execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] bicycle: execute_num: 2 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 1.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] motorcycle: execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 1.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] pedestrian: execute_num: 2 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 1.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] lower_distance_for_polygon_expansion: 0.0 # [m] upper_distance_for_polygon_expansion: 1.0 # [m] diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 4bdcb51f1eab5..799364437955c 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -72,10 +72,12 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); param.envelope_buffer_margin = getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); - param.avoid_margin_lateral = - getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); - param.safety_buffer_lateral = - getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); + param.lateral_soft_margin = + getOrDeclareParameter(*node, ns + "lateral_margin.soft_margin"); + param.lateral_hard_margin = + getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin"); + param.lateral_hard_margin_for_parked_vehicle = + getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle"); return param; }; @@ -130,11 +132,14 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) } { - const std::string ns = "avoidance.target_filtering.force_avoidance."; - p.enable_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "enable"); - p.threshold_time_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "time_threshold"); + const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; + p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "enable"); + p.closest_distance_to_wait_and_see_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "closest_distance_to_wait_and_see"); + p.time_threshold_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "condition.time_threshold"); + p.distance_threshold_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "condition.distance_threshold"); p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); p.object_ignore_section_crosswalk_in_front_distance = diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index fc755bc4115a9..1aab806f76cc9 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -257,9 +257,11 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_ const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification); const auto nearest_object_parameter = avoidance_parameters_->object_parameters.at(nearest_object_type); - const auto avoid_margin = - nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor + - nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width; + const auto lateral_hard_margin = std::max( + nearest_object_parameter.lateral_hard_margin, + nearest_object_parameter.lateral_hard_margin_for_parked_vehicle); + const auto avoid_margin = lateral_hard_margin * nearest_object.distance_factor + + nearest_object_parameter.lateral_soft_margin + 0.5 * ego_width; avoidance_helper_->setData(planner_data_); const auto shift_length = avoidance_helper_->getShiftLength( @@ -288,8 +290,10 @@ double AvoidanceByLaneChange::calcLateralOffset() const { auto additional_lat_offset{0.0}; for (const auto & [type, p] : avoidance_parameters_->object_parameters) { + const auto lateral_hard_margin = + std::max(p.lateral_hard_margin, p.lateral_hard_margin_for_parked_vehicle); const auto offset = - 2.0 * p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral; + 2.0 * p.envelope_buffer_margin + lateral_hard_margin + p.lateral_soft_margin; additional_lat_offset = std::max(additional_lat_offset, offset); } return additional_lat_offset; diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index 5a81d7d972a0e..50b5aef92cf60 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -27,81 +27,97 @@ car: execute_num: 1 # [-] moving_speed_threshold: 1.0 # [m/s] - moving_time_threshold: 1.0 # [s] + moving_time_threshold: 2.0 # [s] + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.3 # [m] + hard_margin_for_parked_vehicle: 0.3 # [m] max_expand_ratio: 0.0 # [-] - envelope_buffer_margin: 0.3 # [m] - avoid_margin_lateral: 1.0 # [m] - safety_buffer_lateral: 0.7 # [m] + envelope_buffer_margin: 0.5 # [m] safety_buffer_longitudinal: 0.0 # [m] use_conservative_buffer_longitudinal: true # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance. truck: execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + moving_time_threshold: 2.0 + lateral_margin: + soft_margin: 0.9 # [m] + hard_margin: 0.1 # [m] + hard_margin_for_parked_vehicle: 0.1 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true bus: execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + moving_time_threshold: 2.0 + lateral_margin: + soft_margin: 0.9 # [m] + hard_margin: 0.1 # [m] + hard_margin_for_parked_vehicle: 0.1 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true trailer: execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + moving_time_threshold: 2.0 + lateral_margin: + soft_margin: 0.9 # [m] + hard_margin: 0.1 # [m] + hard_margin_for_parked_vehicle: 0.1 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true unknown: execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: -0.2 # [m] + hard_margin_for_parked_vehicle: -0.2 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + envelope_buffer_margin: 0.1 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true bicycle: execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.5 # [m] + hard_margin_for_parked_vehicle: 0.5 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.8 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 1.0 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true motorcycle: execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.3 # [m] + hard_margin_for_parked_vehicle: 0.3 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.8 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 1.0 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true pedestrian: execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.5 # [m] + hard_margin_for_parked_vehicle: 0.5 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.8 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 1.0 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true lower_distance_for_polygon_expansion: 30.0 # [m] @@ -137,10 +153,12 @@ backward_distance: 10.0 # [m] # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. - force_avoidance: + avoidance_for_ambiguous_vehicle: enable: false # [-] - time_threshold: 1.0 # [s] - distance_threshold: 1.0 # [m] + closest_distance_to_wait_and_see: 10.0 # [m] + condition: + time_threshold: 1.0 # [s] + distance_threshold: 1.0 # [m] ignore_area: traffic_light: front_distance: 100.0 # [m] diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index e4df3a8069b16..ab79b49d67125 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -69,9 +69,11 @@ struct ObjectParameter double envelope_buffer_margin{0.0}; - double avoid_margin_lateral{1.0}; + double lateral_soft_margin{1.0}; - double safety_buffer_lateral{1.0}; + double lateral_hard_margin{1.0}; + + double lateral_hard_margin_for_parked_vehicle{1.0}; double safety_buffer_longitudinal{0.0}; @@ -98,7 +100,7 @@ struct AvoidanceParameters bool enable_cancel_maneuver{false}; // enable avoidance for all parking vehicle - bool enable_force_avoidance_for_stopped_vehicle{false}; + bool enable_avoidance_for_ambiguous_vehicle{false}; // enable yield maneuver. bool enable_yield_maneuver{false}; @@ -182,8 +184,9 @@ struct AvoidanceParameters double object_check_min_road_shoulder_width{0.0}; // force avoidance - double threshold_time_force_avoidance_for_stopped_vehicle{0.0}; - double force_avoidance_distance_threshold{0.0}; + double closest_distance_to_wait_and_see_for_ambiguous_vehicle{0.0}; + double time_threshold_for_ambiguous_vehicle{0.0}; + double distance_threshold_for_ambiguous_vehicle{0.0}; // when complete avoidance motion, there is a distance margin with the object // for longitudinal direction @@ -382,7 +385,8 @@ struct ObjectData // avoidance target rclcpp::Time last_move; double stop_time{0.0}; - // store the information of the lanelet which the object's overhang is currently occupying + // It is one of the ego driving lanelets (closest lanelet to the object) and used in the logic to + // check whether the object is on the ego lane. lanelet::ConstLanelet overhang_lanelet; // the position at the detected moment @@ -415,6 +419,15 @@ struct ObjectData // avoidance target // is within intersection area bool is_within_intersection{false}; + // is parked vehicle on road shoulder + bool is_parked{false}; + + // is driving on ego current lane + bool is_on_ego_lane{false}; + + // is ambiguous stopped vehicle. + bool is_ambiguous{false}; + // object direction. Direction direction{Direction::NONE}; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index 4f15261f42246..6d2eb81b328f4 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -17,6 +17,7 @@ #include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_avoidance_module/utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include @@ -134,6 +135,24 @@ class AvoidanceHelper shift_length, getLateralMaxJerkLimit(), getAvoidanceEgoSpeed()); } + double getFrontConstantDistance(const ObjectData & object) const + { + const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters_->object_parameters.at(object_type); + const auto & additional_buffer_longitudinal = + object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front + : 0.0; + return object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal; + } + + double getRearConstantDistance(const ObjectData & object) const + { + const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters_->object_parameters.at(object_type); + return object_parameter.safety_buffer_longitudinal + data_->parameters.base_link2rear + + object.length; + } + double getEgoShift() const { validate(); @@ -270,6 +289,35 @@ class AvoidanceHelper }); } + bool isReady(const ObjectDataArray & objects) const + { + if (objects.empty()) { + return true; + } + + const auto object = objects.front(); + + if (!object.is_ambiguous) { + return true; + } + + if (!object.avoid_margin.has_value()) { + return true; + } + + const auto is_object_on_right = utils::avoidance::isOnRight(object); + const auto desire_shift_length = + getShiftLength(object, is_object_on_right, object.avoid_margin.value()); + + const auto prepare_distance = getMinimumPrepareDistance(); + const auto constant_distance = getFrontConstantDistance(object); + const auto avoidance_distance = getMinAvoidanceDistance(desire_shift_length); + + return object.longitudinal < + prepare_distance + constant_distance + avoidance_distance + + parameters_->closest_distance_to_wait_and_see_for_ambiguous_vehicle; + } + bool isReady(const AvoidLineArray & new_shift_lines, const double current_shift_length) const { if (std::abs(current_shift_length) < 1e-3) { diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index ce75fa03e96ad..d23f5c8729edd 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -72,10 +72,12 @@ AvoidanceParameters getParameter(rclcpp::Node * node) param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); param.envelope_buffer_margin = getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); - param.avoid_margin_lateral = - getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); - param.safety_buffer_lateral = - getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); + param.lateral_soft_margin = + getOrDeclareParameter(*node, ns + "lateral_margin.soft_margin"); + param.lateral_hard_margin = + getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin"); + param.lateral_hard_margin_for_parked_vehicle = + getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle"); param.safety_buffer_longitudinal = getOrDeclareParameter(*node, ns + "safety_buffer_longitudinal"); param.use_conservative_buffer_longitudinal = @@ -138,13 +140,14 @@ AvoidanceParameters getParameter(rclcpp::Node * node) } { - const std::string ns = "avoidance.target_filtering.force_avoidance."; - p.enable_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "enable"); - p.threshold_time_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "time_threshold"); - p.force_avoidance_distance_threshold = - getOrDeclareParameter(*node, ns + "distance_threshold"); + const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; + p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "enable"); + p.closest_distance_to_wait_and_see_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "closest_distance_to_wait_and_see"); + p.time_threshold_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "condition.time_threshold"); + p.distance_threshold_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "condition.distance_threshold"); p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); p.object_ignore_section_crosswalk_in_front_distance = diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index 2d61571392eef..9dc23ea4ec80e 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -176,9 +176,6 @@ double calcDistanceToAvoidStartLine( const std::shared_ptr & planner_data, const std::shared_ptr & parameters); -std::pair calcTurnSignalInfo( - const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length, - const AvoidancePlanningData & data, const std::shared_ptr & planner_data); } // namespace behavior_path_planner::utils::avoidance #endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index cc7870375d0db..7d029277fcbc4 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -176,7 +176,7 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st marker.id = uuidToInt32(object.object.object_id); marker.pose.position.z += 2.0; std::ostringstream string_stream; - string_stream << object.reason; + string_stream << object.reason << (object.is_parked ? "(PARKED)" : ""); marker.text = string_stream.str(); marker.color = createMarkerColor(1.0, 1.0, 1.0, 0.999); marker.scale = createMarkerScale(0.6, 0.6, 0.6); @@ -188,10 +188,25 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st return msg; } +MarkerArray createOverhangLaneletMarkerArray(const ObjectDataArray & objects, std::string && ns) +{ + MarkerArray msg; + msg.markers.reserve(objects.size()); + + for (const auto & object : objects) { + appendMarkerArray( + marker_utils::createLaneletsAreaMarkerArray( + {object.overhang_lanelet}, std::string(ns), 0.0, 0.0, 1.0), + &msg); + } + + return msg; +} + MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns) { MarkerArray msg; - msg.markers.reserve(objects.size() * 4); + msg.markers.reserve(objects.size() * 5); appendMarkerArray( createObjectsCubeMarkerArray( @@ -202,6 +217,7 @@ MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::st appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); + appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg); return msg; } @@ -209,7 +225,7 @@ MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::st MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns) { MarkerArray msg; - msg.markers.reserve(objects.size() * 4); + msg.markers.reserve(objects.size() * 5); appendMarkerArray( createObjectsCubeMarkerArray( @@ -220,6 +236,7 @@ MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std:: appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); + appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg); return msg; } @@ -451,6 +468,10 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const appendMarkerArray( createObjectInfoMarkerArray(filtered_objects, "others_" + convertToSnakeCase(ns) + "_info"), &msg); + appendMarkerArray( + createOverhangLaneletMarkerArray( + filtered_objects, "others_" + convertToSnakeCase(ns) + "_overhang_lanelet"), + &msg); return msg; } @@ -558,6 +579,8 @@ MarkerArray createDebugMarkerArray( addObjects(data.other_objects, std::string("ParallelToEgoLane")); addObjects(data.other_objects, std::string("MergingToEgoLane")); addObjects(data.other_objects, std::string("UnstableObject")); + addObjects(data.other_objects, std::string("AmbiguousStoppedVehicle")); + addObjects(data.other_objects, std::string("AmbiguousStoppedVehicle(wait-and-see)")); } // shift line pre-process diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp index 68cee1aa2b523..692e4260520f3 100644 --- a/planning/behavior_path_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -60,8 +60,11 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "moving_time_threshold", config.moving_time_threshold); updateParam(parameters, ns + "max_expand_ratio", config.max_expand_ratio); updateParam(parameters, ns + "envelope_buffer_margin", config.envelope_buffer_margin); - updateParam(parameters, ns + "avoid_margin_lateral", config.avoid_margin_lateral); - updateParam(parameters, ns + "safety_buffer_lateral", config.safety_buffer_lateral); + updateParam(parameters, ns + "lateral_margin.soft_margin", config.lateral_soft_margin); + updateParam(parameters, ns + "lateral_margin.hard_margin", config.lateral_hard_margin); + updateParam( + parameters, ns + "lateral_margin.hard_margin_for_parked_vehicle", + config.lateral_hard_margin_for_parked_vehicle); updateParam( parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal); updateParam( diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index ed3dc8d34a698..0ecaf76e8c2d1 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -321,10 +321,19 @@ void AvoidanceModule::fillAvoidanceTargetObjects( using utils::avoidance::getTargetLanelets; using utils::avoidance::separateObjectsByPath; using utils::avoidance::updateRoadShoulderDistance; + using utils::traffic_light::calcDistanceToRedTrafficLight; // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. constexpr double MARGIN = 10.0; - const auto forward_detection_range = helper_->getForwardDetectionRange(); + const auto forward_detection_range = [&]() { + const auto to_traffic_light = calcDistanceToRedTrafficLight( + data.current_lanelets, helper_->getPreviousReferencePath(), planner_data_); + if (!to_traffic_light.has_value()) { + return helper_->getForwardDetectionRange(); + } + return std::min(helper_->getForwardDetectionRange(), to_traffic_light.value()); + }(); + const auto [object_within_target_lane, object_outside_target_lane] = separateObjectsByPath( helper_->getPreviousReferencePath(), helper_->getPreviousSplineShiftPath().path, planner_data_, data, parameters_, forward_detection_range + MARGIN, debug); @@ -513,7 +522,8 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de */ data.comfortable = helper_->isComfortable(data.new_shift_line); data.safe = isSafePath(data.candidate_path, debug); - data.ready = helper_->isReady(data.new_shift_line, path_shifter_.getLastShiftLength()); + data.ready = helper_->isReady(data.new_shift_line, path_shifter_.getLastShiftLength()) && + helper_->isReady(data.target_objects); } void AvoidanceModule::fillEgoStatus( @@ -629,21 +639,16 @@ void AvoidanceModule::fillDebugData( const auto o_front = data.stop_target_object.value(); const auto object_type = utils::getHighestProbLabel(o_front.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto & additional_buffer_longitudinal = - object_parameter.use_conservative_buffer_longitudinal - ? planner_data_->parameters.base_link2front - : 0.0; + const auto constant_distance = helper_->getFrontConstantDistance(o_front); const auto & vehicle_width = planner_data_->parameters.vehicle_width; - const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o_front.distance_factor + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; + const auto max_avoid_margin = object_parameter.lateral_hard_margin * o_front.distance_factor + + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; - const auto variable = helper_->getSharpAvoidanceDistance( + const auto avoidance_distance = helper_->getSharpAvoidanceDistance( helper_->getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin)); - const auto constant = helper_->getNominalPrepareDistance() + - object_parameter.safety_buffer_longitudinal + - additional_buffer_longitudinal; - const auto total_avoid_distance = variable + constant; + const auto prepare_distance = helper_->getNominalPrepareDistance(); + const auto total_avoid_distance = prepare_distance + avoidance_distance + constant_distance; dead_pose_ = calcLongitudinalOffsetPose( data.reference_path.points, getEgoPosition(), o_front.longitudinal - total_avoid_distance); @@ -938,9 +943,13 @@ BehaviorModuleOutput AvoidanceModule::plan() output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; } else { const auto original_signal = getPreviousModuleOutput().turn_signal_info; - const auto [new_signal, is_ignore] = utils::avoidance::calcTurnSignalInfo( - linear_shift_path, path_shifter_.getShiftLines().front(), helper_->getEgoShift(), avoid_data_, - planner_data_); + + constexpr bool is_driving_forward = true; + constexpr bool egos_lane_is_shifted = true; + const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( + linear_shift_path, path_shifter_.getShiftLines().front(), avoid_data_.current_lanelets, + helper_->getEgoShift(), is_driving_forward, egos_lane_is_shifted); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points); output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal, @@ -1423,18 +1432,16 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; - const auto variable = helper_->getMinAvoidanceDistance( + const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor + + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; + const auto avoidance_distance = helper_->getMinAvoidanceDistance( helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); - const auto & additional_buffer_longitudinal = - object_parameter.use_conservative_buffer_longitudinal - ? planner_data_->parameters.base_link2front - : 0.0; - const auto constant = p->min_prepare_distance + object_parameter.safety_buffer_longitudinal + - additional_buffer_longitudinal + p->stop_buffer; - - return object.longitudinal - std::min(variable + constant, p->stop_max_distance); + const auto constant_distance = helper_->getFrontConstantDistance(object); + + return object.longitudinal - + std::min( + avoidance_distance + constant_distance + p->min_prepare_distance + p->stop_buffer, + p->stop_max_distance); } void AvoidanceModule::insertReturnDeadLine( @@ -1653,8 +1660,8 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const const auto & vehicle_width = planner_data_->parameters.vehicle_width; const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; + const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor + + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; const auto shift_length = helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin); diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index cd2a08489fd29..5a8fd35f6d618 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -123,7 +123,6 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( { // To be consistent with changes in the ego position, the current shift length is considered. const auto current_ego_shift = helper_->getEgoShift(); - const auto & base_link2rear = data_->parameters.base_link2rear; // Calculate feasible shift length const auto get_shift_profile = @@ -140,13 +139,10 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( // calculate remaining distance. const auto prepare_distance = helper_->getNominalPrepareDistance(); - const auto & additional_buffer_longitudinal = - object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front - : 0.0; - const auto constant = object_parameter.safety_buffer_longitudinal + - additional_buffer_longitudinal + prepare_distance; - const auto has_enough_distance = object.longitudinal > constant + nominal_avoid_distance; - const auto remaining_distance = object.longitudinal - constant; + const auto constant_distance = helper_->getFrontConstantDistance(object); + const auto has_enough_distance = + object.longitudinal > constant_distance + prepare_distance + nominal_avoid_distance; + const auto remaining_distance = object.longitudinal - constant_distance - prepare_distance; const auto avoidance_distance = has_enough_distance ? nominal_avoid_distance : remaining_distance; @@ -234,9 +230,12 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( const double LAT_DIST_BUFFER = desire_shift_length > 0.0 ? 1e-3 : -1e-3; + const auto lateral_hard_margin = object.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; const auto infeasible = std::abs(feasible_shift_length - object.overhang_points.front().first) - LAT_DIST_BUFFER < - 0.5 * data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral; + 0.5 * data_->parameters.vehicle_width + lateral_hard_margin; if (infeasible) { RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. "); object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; @@ -275,11 +274,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } } - // use each object param - const auto object_type = utils::getHighestProbLabel(o.object.classification); - const auto object_parameter = parameters_->object_parameters.at(object_type); + // calculate feasible shift length based on behavior policy const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length); - if (!feasible_shift_profile.has_value()) { if (o.avoid_required && is_forward_object(o)) { break; @@ -294,12 +290,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( AvoidLine al_avoid; { - const auto & additional_buffer_longitudinal = - object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front - : 0.0; - const auto offset = - object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal; - const auto to_shift_end = o.longitudinal - offset; + const auto constant_distance = helper_->getFrontConstantDistance(o); + const auto to_shift_end = o.longitudinal - constant_distance; const auto path_front_to_ego = data.arclength_from_ego.at(data.ego_closest_path_index); // start point (use previous linear shift length as start shift length.) @@ -335,8 +327,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( AvoidLine al_return; { - const auto offset = object_parameter.safety_buffer_longitudinal + base_link2rear + o.length; - const auto to_shift_start = o.longitudinal + offset; + const auto constant_distance = helper_->getRearConstantDistance(o); + const auto to_shift_start = o.longitudinal + constant_distance; // start point al_return.start_shift_length = feasible_shift_profile.value().first; diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index df775ea6b2c0a..fdb2b9b71c782 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -250,132 +250,6 @@ void pushUniqueVector(T & base_vector, const T & additional_vector) base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end()); } -bool isAvoidShift( - const double start_shift_length, const double end_shift_length, const double threshold) -{ - return std::abs(start_shift_length) < threshold && std::abs(end_shift_length) > threshold; -} - -bool isReturnShift( - const double start_shift_length, const double end_shift_length, const double threshold) -{ - return std::abs(start_shift_length) > threshold && std::abs(end_shift_length) < threshold; -} - -bool isLeftMiddleShift( - const double start_shift_length, const double end_shift_length, const double threshold) -{ - return start_shift_length > threshold && end_shift_length > threshold; -} - -bool isRightMiddleShift( - const double start_shift_length, const double end_shift_length, const double threshold) -{ - return start_shift_length < threshold && end_shift_length < threshold; -} - -bool existShiftSideLane( - const double start_shift_length, const double end_shift_length, const bool no_left_lanes, - const bool no_right_lanes, const double threshold) -{ - const auto relative_shift_length = end_shift_length - start_shift_length; - - if (isAvoidShift(start_shift_length, end_shift_length, threshold)) { - // Left avoid. But there is no adjacent lane. No need blinker. - if (relative_shift_length > 0.0 && no_left_lanes) { - return false; - } - - // Right avoid. But there is no adjacent lane. No need blinker. - if (relative_shift_length < 0.0 && no_right_lanes) { - return false; - } - } - - if (isReturnShift(start_shift_length, end_shift_length, threshold)) { - // Right return. But there is no adjacent lane. No need blinker. - if (relative_shift_length > 0.0 && no_right_lanes) { - return false; - } - - // Left return. But there is no adjacent lane. No need blinker. - if (relative_shift_length < 0.0 && no_left_lanes) { - return false; - } - } - - if (isLeftMiddleShift(start_shift_length, end_shift_length, threshold)) { - // Left avoid. But there is no adjacent lane. No need blinker. - if (relative_shift_length > 0.0 && no_left_lanes) { - return false; - } - - // Left return. But there is no adjacent lane. No need blinker. - if (relative_shift_length < 0.0 && no_left_lanes) { - return false; - } - } - - if (isRightMiddleShift(start_shift_length, end_shift_length, threshold)) { - // Right avoid. But there is no adjacent lane. No need blinker. - if (relative_shift_length < 0.0 && no_right_lanes) { - return false; - } - - // Left avoid. But there is no adjacent lane. No need blinker. - if (relative_shift_length > 0.0 && no_right_lanes) { - return false; - } - } - - return true; -} - -bool isNearEndOfShift( - const double start_shift_length, const double end_shift_length, const Point & ego_pos, - const lanelet::ConstLanelets & original_lanes, const double threshold) -{ - using boost::geometry::within; - using lanelet::utils::to2D; - using lanelet::utils::conversion::toLaneletPoint; - - if (!isReturnShift(start_shift_length, end_shift_length, threshold)) { - return false; - } - - return std::any_of(original_lanes.begin(), original_lanes.end(), [&ego_pos](const auto & lane) { - return within(to2D(toLaneletPoint(ego_pos)), lane.polygon2d().basicPolygon()); - }); -} - -bool straddleRoadBound( - const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes, - const vehicle_info_util::VehicleInfo & vehicle_info) -{ - using boost::geometry::intersects; - using tier4_autoware_utils::pose2transform; - using tier4_autoware_utils::transformVector; - - const auto footprint = vehicle_info.createFootprint(); - - for (const auto & lane : lanes) { - for (size_t i = shift_line.start_idx; i < shift_line.end_idx; ++i) { - const auto transform = pose2transform(path.path.points.at(i).point.pose); - const auto shifted_vehicle_footprint = transformVector(footprint, transform); - - if (intersects(lane.leftBound2d().basicLineString(), shifted_vehicle_footprint)) { - return true; - } - - if (intersects(lane.rightBound2d().basicLineString(), shifted_vehicle_footprint)) { - return true; - } - } - } - - return false; -} - } // namespace namespace filtering_utils @@ -478,6 +352,14 @@ bool isWithinIntersection( object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon()))); } +bool isOnEgoLane(const ObjectData & object) +{ + const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; + return boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), + object.overhang_lanelet.polygon2d().basicPolygon()); +} + bool isParallelToEgoLane(const ObjectData & object, const double threshold) { const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; @@ -516,35 +398,23 @@ bool isMergingToEgoLane(const ObjectData & object) return true; } -bool isObjectOnRoadShoulder( - ObjectData & object, const std::shared_ptr & route_handler, +bool isParkedVehicle( + ObjectData & object, const AvoidancePlanningData & data, + const std::shared_ptr & route_handler, const std::shared_ptr & parameters) { - using boost::geometry::within; using lanelet::geometry::distance2d; using lanelet::geometry::toArcCoordinates; using lanelet::utils::to2D; using lanelet::utils::conversion::toLaneletPoint; - // assume that there are no parked vehicles in intersection. - std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else"); - if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { + if (object.is_within_intersection) { return false; } - // ============================================ <- most_left_lanelet.leftBound() - // y road shoulder - // ^ ------------------------------------------ - // | x + - // +---> --- object closest lanelet --- o ----- <- object_closest_lanelet.centerline() - // - // -------------------------------------------- - // +: object position - // o: nearest point on centerline - - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; const auto centerline_pos = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position).position; + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pos).position; bool is_left_side_parked_vehicle = false; if (!isOnRight(object)) { @@ -580,7 +450,7 @@ bool isObjectOnRoadShoulder( const auto arc_coordinates = toArcCoordinates( to2D(object.overhang_lanelet.centerline().basicLineString()), - to2D(toLaneletPoint(object_pose.position)).basicPoint()); + to2D(toLaneletPoint(object_pos)).basicPoint()); object.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance; is_left_side_parked_vehicle = object.shiftable_ratio > parameters->object_check_shiftable_ratio; @@ -620,64 +490,43 @@ bool isObjectOnRoadShoulder( const auto arc_coordinates = toArcCoordinates( to2D(object.overhang_lanelet.centerline().basicLineString()), - to2D(toLaneletPoint(object_pose.position)).basicPoint()); + to2D(toLaneletPoint(object_pos)).basicPoint()); object.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance; is_right_side_parked_vehicle = object.shiftable_ratio > parameters->object_check_shiftable_ratio; } - return is_left_side_parked_vehicle || is_right_side_parked_vehicle; -} - -bool isForceAvoidanceTarget( - ObjectData & object, const AvoidancePlanningData & data, - const std::shared_ptr & planner_data, - const std::shared_ptr & parameters) -{ - if (!parameters->enable_force_avoidance_for_stopped_vehicle) { - return false; - } - - const auto stop_time_longer_than_threshold = - object.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle; - - if (!stop_time_longer_than_threshold) { + if (!is_left_side_parked_vehicle && !is_right_side_parked_vehicle) { return false; } const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; - const auto is_moving_distance_longer_than_threshold = - tier4_autoware_utils::calcDistance2d(object.init_pose, object_pose) > - parameters->force_avoidance_distance_threshold; - - if (is_moving_distance_longer_than_threshold) { - return false; - } - - if (object.is_within_intersection) { - RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is in the intersection area."); + object.to_centerline = + lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { return false; } - const auto rh = planner_data->route_handler; - - if ( - !!rh->getRoutingGraphPtr()->right(object.overhang_lanelet) && - !!rh->getRoutingGraphPtr()->left(object.overhang_lanelet)) { - RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object isn't on the edge lane."); - return false; - } + return true; +} +bool isCloseToStopFactor( + ObjectData & object, const AvoidancePlanningData & data, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + const auto & rh = planner_data->route_handler; const auto & ego_pose = planner_data->self_odometry->pose.pose; + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; // force avoidance for stopped vehicle - bool not_parked_object = true; + bool is_close_to_stop_factor = false; // check traffic light const auto to_traffic_light = getDistanceToNextTrafficLight(object_pose, data.extend_lanelets); { - not_parked_object = + is_close_to_stop_factor = to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance; } @@ -689,12 +538,89 @@ bool isForceAvoidanceTarget( const auto stop_for_crosswalk = to_crosswalk < parameters->object_ignore_section_crosswalk_in_front_distance && to_crosswalk > -1.0 * parameters->object_ignore_section_crosswalk_behind_distance; - not_parked_object = not_parked_object || stop_for_crosswalk; + is_close_to_stop_factor = is_close_to_stop_factor || stop_for_crosswalk; } object.to_stop_factor_distance = std::min(to_traffic_light, to_crosswalk); - return !not_parked_object; + return is_close_to_stop_factor; +} + +bool isNeverAvoidanceTarget( + ObjectData & object, const AvoidancePlanningData & data, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto is_moving_distance_longer_than_threshold = + tier4_autoware_utils::calcDistance2d(object.init_pose, object_pose) > + parameters->distance_threshold_for_ambiguous_vehicle; + if (is_moving_distance_longer_than_threshold) { + object.reason = AvoidanceDebugFactor::MOVING_OBJECT; + return true; + } + + if (object.is_within_intersection) { + if (object.behavior == ObjectData::Behavior::NONE) { + object.reason = "ParallelToEgoLane"; + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object belongs to ego lane. never avoid it."); + return true; + } + + if (object.behavior == ObjectData::Behavior::MERGING) { + object.reason = "MergingToEgoLane"; + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object belongs to ego lane. never avoid it."); + return true; + } + } + + if (object.is_on_ego_lane) { + if ( + planner_data->route_handler->getRightLanelet(object.overhang_lanelet).has_value() && + planner_data->route_handler->getLeftLanelet(object.overhang_lanelet).has_value()) { + object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object isn't on the edge lane. never avoid it."); + return true; + } + } + + if (isCloseToStopFactor(object, data, planner_data, parameters)) { + if (object.is_on_ego_lane && !object.is_parked) { + object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is close to stop factor. never avoid it."); + return true; + } + } + + return false; +} + +bool isObviousAvoidanceTarget( + ObjectData & object, [[maybe_unused]] const AvoidancePlanningData & data, + [[maybe_unused]] const std::shared_ptr & planner_data, + [[maybe_unused]] const std::shared_ptr & parameters) +{ + if (!object.is_within_intersection) { + if (object.is_parked && object.behavior == ObjectData::Behavior::NONE) { + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is obvious parked vehicle."); + return true; + } + + if (!object.is_on_ego_lane && object.behavior == ObjectData::Behavior::NONE) { + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is adjacent vehicle."); + return true; + } + } + + if (!object.is_parked) { + object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + } + + if (object.behavior == ObjectData::Behavior::MERGING) { + object.reason = "MergingToEgoLane"; + } + + return false; } bool isSatisfiedWithCommonCondition( @@ -795,59 +721,58 @@ bool isSatisfiedWithVehicleCondition( const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { - using boost::geometry::within; - using lanelet::utils::to2D; - using lanelet::utils::conversion::toLaneletPoint; - object.behavior = getObjectBehavior(object, parameters); - object.is_within_intersection = isWithinIntersection(object, planner_data->route_handler); + object.is_on_ego_lane = isOnEgoLane(object); - // from here condition check for vehicle type objects. - if (isForceAvoidanceTarget(object, data, planner_data, parameters)) { + if (isNeverAvoidanceTarget(object, data, planner_data, parameters)) { + return false; + } + + if (isObviousAvoidanceTarget(object, data, planner_data, parameters)) { return true; } - // check vehicle shift ratio - const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; - const auto on_ego_driving_lane = within( - to2D(toLaneletPoint(object_pos)).basicPoint(), - object.overhang_lanelet.polygon2d().basicPolygon()); - if (on_ego_driving_lane) { - if (isObjectOnRoadShoulder(object, planner_data->route_handler, parameters)) { - return true; - } else { - object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; - return false; - } + // from here, filtering for ambiguous vehicle. + + if (!parameters->enable_avoidance_for_ambiguous_vehicle) { + object.reason = "AmbiguousStoppedVehicle"; + return false; } - // Object is on center line -> ignore. - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; - object.to_centerline = - lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; - if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { - object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; + const auto stop_time_longer_than_threshold = + object.stop_time > parameters->time_threshold_for_ambiguous_vehicle; + if (!stop_time_longer_than_threshold) { + object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; return false; } if (object.is_within_intersection) { - std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else"); - if (turn_direction == "straight") { + if (object.behavior == ObjectData::Behavior::DEVIATING) { + object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; + object.is_ambiguous = true; + return true; + } + } else { + if (object.behavior == ObjectData::Behavior::MERGING) { + object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; + object.is_ambiguous = true; return true; } - if (object.behavior == ObjectData::Behavior::NONE) { - object.reason = "ParallelToEgoLane"; - return false; + if (object.behavior == ObjectData::Behavior::DEVIATING) { + object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; + object.is_ambiguous = true; + return true; } - } - if (object.behavior == ObjectData::Behavior::MERGING) { - object.reason = "MergingToEgoLane"; - return false; + if (object.behavior == ObjectData::Behavior::NONE) { + object.is_ambiguous = false; + return true; + } } - return true; + object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + return false; } bool isNoNeedAvoidanceBehavior( @@ -879,10 +804,13 @@ std::optional getAvoidMargin( const auto & vehicle_width = planner_data->parameters.vehicle_width; const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters->object_parameters.at(object_type); + const auto lateral_hard_margin = object.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; - const auto max_avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; - const auto min_avoid_margin = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width; + const auto max_avoid_margin = lateral_hard_margin * object.distance_factor + + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; + const auto min_avoid_margin = lateral_hard_margin + 0.5 * vehicle_width; const auto soft_lateral_distance_limit = object.to_road_shoulder_distance - parameters->soft_road_shoulder_margin - 0.5 * vehicle_width; const auto hard_lateral_distance_limit = @@ -1522,8 +1450,11 @@ void fillAvoidanceNecessity( { const auto object_type = utils::getHighestProbLabel(object_data.object.classification); const auto object_parameter = parameters->object_parameters.at(object_type); + const auto lateral_hard_margin = object_data.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; const auto safety_margin = - 0.5 * vehicle_width + object_parameter.safety_buffer_lateral * object_data.distance_factor; + 0.5 * vehicle_width + lateral_hard_margin * object_data.distance_factor; const auto check_necessity = [&](const auto hysteresis_factor) { return (isOnRight(object_data) && std::abs(object_data.overhang_points.front().first) < @@ -1706,8 +1637,11 @@ void updateRoadShoulderDistance( const auto & vehicle_width = planner_data->parameters.vehicle_width; const auto object_type = utils::getHighestProbLabel(o.object.classification); const auto object_parameter = parameters->object_parameters.at(object_type); + const auto lateral_hard_margin = o.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; - o.avoid_margin = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width; + o.avoid_margin = lateral_hard_margin + 0.5 * vehicle_width; } const auto extract_obstacles = generateObstaclePolygonsForDrivableArea( clip_objects, parameters, planner_data->parameters.vehicle_width / 2.0); @@ -1751,7 +1685,6 @@ void filterTargetObjects( // Find the footprint point closest to the path, set to object_data.overhang_distance. o.overhang_points = utils::avoidance::calcEnvelopeOverhangDistance(o, data.reference_path); o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data); - o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); // TODO(Satoshi Ota) parametrize stop time threshold if need. constexpr double STOP_TIME_THRESHOLD = 3.0; // [s] @@ -1763,17 +1696,31 @@ void filterTargetObjects( } } - if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { - data.other_objects.push_back(o); - continue; - } - if (filtering_utils::isVehicleTypeObject(o)) { + o.is_within_intersection = + filtering_utils::isWithinIntersection(o, planner_data->route_handler); + o.is_parked = + filtering_utils::isParkedVehicle(o, data, planner_data->route_handler, parameters); + o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); + + if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { + data.other_objects.push_back(o); + continue; + } + if (!filtering_utils::isSatisfiedWithVehicleCondition(o, data, planner_data, parameters)) { data.other_objects.push_back(o); continue; } } else { + o.is_parked = false; + o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); + + if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { + data.other_objects.push_back(o); + continue; + } + if (!filtering_utils::isSatisfiedWithNonVehicleCondition(o, data, planner_data, parameters)) { data.other_objects.push_back(o); continue; @@ -2081,8 +2028,10 @@ std::pair separateObjectsByPath( double max_offset = 0.0; for (const auto & object_parameter : parameters->object_parameters) { const auto p = object_parameter.second; + const auto lateral_hard_margin = + std::max(p.lateral_hard_margin, p.lateral_hard_margin_for_parked_vehicle); const auto offset = - 2.0 * p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral; + 2.0 * p.envelope_buffer_margin + lateral_hard_margin + p.lateral_soft_margin; max_offset = std::max(max_offset, offset); } @@ -2356,115 +2305,4 @@ double calcDistanceToReturnDeadLine( return distance_to_return_dead_line; } - -std::pair calcTurnSignalInfo( - const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length, - const AvoidancePlanningData & data, const std::shared_ptr & planner_data) -{ - constexpr double THRESHOLD = 0.1; - const auto & p = planner_data->parameters; - const auto & rh = planner_data->route_handler; - const auto & ego_pose = planner_data->self_odometry->pose.pose; - const auto & ego_speed = planner_data->self_odometry->twist.twist.linear.x; - - if (shift_line.start_idx + 1 > path.shift_length.size()) { - RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); - return std::make_pair(TurnSignalInfo{}, true); - } - - if (shift_line.start_idx + 1 > path.path.points.size()) { - RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); - return std::make_pair(TurnSignalInfo{}, true); - } - - if (shift_line.end_idx + 1 > path.shift_length.size()) { - RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); - return std::make_pair(TurnSignalInfo{}, true); - } - - if (shift_line.end_idx + 1 > path.path.points.size()) { - RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); - return std::make_pair(TurnSignalInfo{}, true); - } - - const auto start_shift_length = path.shift_length.at(shift_line.start_idx); - const auto end_shift_length = path.shift_length.at(shift_line.end_idx); - const auto relative_shift_length = end_shift_length - start_shift_length; - - // If shift length is shorter than the threshold, it does not need to turn on blinkers - if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) { - return std::make_pair(TurnSignalInfo{}, true); - } - - // If the vehicle does not shift anymore, we turn off the blinker - if (std::fabs(path.shift_length.at(shift_line.end_idx) - current_shift_length) < THRESHOLD) { - return std::make_pair(TurnSignalInfo{}, true); - } - - const auto get_command = [](const auto & shift_length) { - return shift_length > 0.0 ? TurnIndicatorsCommand::ENABLE_LEFT - : TurnIndicatorsCommand::ENABLE_RIGHT; - }; - - const auto signal_prepare_distance = - std::max(ego_speed * p.turn_signal_search_time, p.turn_signal_minimum_search_distance); - const auto ego_front_to_shift_start = - calcSignedArcLength(path.path.points, ego_pose.position, shift_line.start_idx) - - p.vehicle_info.max_longitudinal_offset_m; - - if (signal_prepare_distance < ego_front_to_shift_start) { - return std::make_pair(TurnSignalInfo{}, false); - } - - const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose; - const auto blinker_end_pose = path.path.points.at(shift_line.end_idx).point.pose; - const auto get_start_pose = [&](const auto & ego_to_shift_start) { - return ego_to_shift_start ? ego_pose : blinker_start_pose; - }; - - TurnSignalInfo turn_signal_info{}; - turn_signal_info.desired_start_point = get_start_pose(ego_front_to_shift_start); - turn_signal_info.desired_end_point = blinker_end_pose; - turn_signal_info.required_start_point = blinker_start_pose; - turn_signal_info.required_end_point = blinker_end_pose; - turn_signal_info.turn_signal.command = get_command(relative_shift_length); - - if (!p.turn_signal_on_swerving) { - return std::make_pair(turn_signal_info, false); - } - - lanelet::ConstLanelet lanelet; - if (!rh->getClosestLaneletWithinRoute(shift_line.end, &lanelet)) { - return std::make_pair(TurnSignalInfo{}, true); - } - - const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true); - const auto left_opposite_lanes = rh->getLeftOppositeLanelets(lanelet); - const auto right_same_direction_lane = rh->getRightLanelet(lanelet, true, true); - const auto right_opposite_lanes = rh->getRightOppositeLanelets(lanelet); - const auto has_left_lane = left_same_direction_lane.has_value() || !left_opposite_lanes.empty(); - const auto has_right_lane = - right_same_direction_lane.has_value() || !right_opposite_lanes.empty(); - - if (!existShiftSideLane( - start_shift_length, end_shift_length, !has_left_lane, !has_right_lane, - p.turn_signal_shift_length_threshold)) { - return std::make_pair(TurnSignalInfo{}, true); - } - - if (!straddleRoadBound(path, shift_line, data.current_lanelets, p.vehicle_info)) { - return std::make_pair(TurnSignalInfo{}, true); - } - - constexpr double STOPPED_THRESHOLD = 0.1; // [m/s] - if (ego_speed < STOPPED_THRESHOLD) { - if (isNearEndOfShift( - start_shift_length, end_shift_length, ego_pose.position, data.current_lanelets, - p.turn_signal_shift_length_threshold)) { - return std::make_pair(TurnSignalInfo{}, true); - } - } - - return std::make_pair(turn_signal_info, false); -} } // namespace behavior_path_planner::utils::avoidance diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index ce0fc8cfbece1..c2902fb0bace6 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1856,6 +1856,8 @@ bool GoalPlannerModule::isCrossingPossible( Pose end_lane_pose{}; end_lane_pose.orientation.w = 1.0; end_lane_pose.position = lanelet::utils::conversion::toGeomMsgPt(end_lane.centerline().front()); + // NOTE: this line does not specify the /forward/backward length, so if the shoulders form a + // loop, this returns all shoulder lanes in the loop end_lane_sequence = route_handler->getShoulderLaneletSequence(end_lane, end_lane_pose); } else { const double dist = std::numeric_limits::max(); diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 454282d4eacd3..3b8d2ad6e9931 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -151,8 +151,6 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; - TurnSignalInfo calcTurnSignalInfo() const override; - bool isValidPath(const PathWithLaneId & path) const override; PathSafetyStatus isLaneChangePathSafe( diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index 3bb250c5affd8..3efc0c9dd61dc 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -218,8 +218,6 @@ class LaneChangeBase LaneChangePaths * candidate_paths, const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety) const = 0; - virtual TurnSignalInfo calcTurnSignalInfo() const = 0; - virtual bool isValidPath(const PathWithLaneId & path) const = 0; virtual bool isAbleToStopSafely() const = 0; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index 46b47de128bd9..35cc02e867557 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -141,10 +141,6 @@ bool hasEnoughLengthToLaneChangeAfterAbort( const Pose & curent_pose, const double abort_return_dist, const LaneChangeParameters & lane_change_parameters, const Direction direction); -lanelet::ConstLanelets getBackwardLanelets( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & current_pose, const double backward_length); - double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0); double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer); diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 2021630f1ae34..03569aa4f3cdd 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -480,14 +480,20 @@ void NormalLaneChange::resetParameters() TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const { - TurnSignalInfo turn_signal_info = calcTurnSignalInfo(); - const auto [turn_signal_command, distance_to_vehicle_front] = utils::getPathTurnSignal( - status_.current_lanes, status_.lane_change_path.shifted_path, - status_.lane_change_path.info.shift_line, getEgoPose(), getEgoTwist().linear.x, - planner_data_->parameters); - turn_signal_info.turn_signal.command = turn_signal_command.command; - - return turn_signal_info; + const auto & pose = getEgoPose(); + const auto & current_lanes = status_.current_lanes; + const auto & shift_line = status_.lane_change_path.info.shift_line; + const auto & shift_path = status_.lane_change_path.shifted_path; + const auto current_shift_length = lanelet::utils::getArcCoordinates(current_lanes, pose).distance; + constexpr bool is_driving_forward = true; + // The getBehaviorTurnSignalInfo method expects the shifted line to be generated off of the ego's + // current lane, lane change is different, so we set this flag to false. + constexpr bool egos_lane_is_shifted = false; + + const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( + shift_path, shift_line, current_lanes, current_shift_length, is_driving_forward, + egos_lane_is_shifted); + return new_signal; } lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const @@ -843,7 +849,7 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( // get backward lanes const auto backward_length = lane_change_parameters_->backward_lane_length; - const auto target_backward_lanes = utils::lane_change::getBackwardLanelets( + const auto target_backward_lanes = behavior_path_planner::utils::getBackwardLanelets( route_handler, target_lanes, current_pose, backward_length); // filter objects to get target object index @@ -1247,9 +1253,9 @@ bool NormalLaneChange::getLaneChangePaths( }; // get path on original lanes - const auto prepare_velocity = std::max( + const auto prepare_velocity = std::clamp( current_velocity + sampled_longitudinal_acc * prepare_duration, - minimum_lane_changing_velocity); + minimum_lane_changing_velocity, getCommonParam().max_vel); // compute actual longitudinal acceleration const double longitudinal_acc_on_prepare = @@ -1313,8 +1319,9 @@ bool NormalLaneChange::getLaneChangePaths( const auto lane_changing_length = initial_lane_changing_velocity * lane_changing_time + 0.5 * longitudinal_acc_on_lane_changing * lane_changing_time * lane_changing_time; - const auto terminal_lane_changing_velocity = - initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time; + const auto terminal_lane_changing_velocity = std::min( + initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time, + getCommonParam().max_vel); utils::lane_change::setPrepareVelocity( prepare_segment, current_velocity, terminal_lane_changing_velocity); @@ -1686,59 +1693,6 @@ PathSafetyStatus NormalLaneChange::evaluateApprovedPathWithUnsafeHysteresis( return {}; } -TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() const -{ - const auto get_blinker_pose = [](const PathWithLaneId & path, const double length) { - double accumulated_length = 0.0; - for (size_t i = 0; i < path.points.size() - 1; ++i) { - accumulated_length += - tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); - if (accumulated_length > length) { - return path.points.at(i).point.pose; - } - } - - return path.points.front().point.pose; - }; - - const auto & path = status_.lane_change_path; - const auto & shifted_path = path.shifted_path.path; - - TurnSignalInfo turn_signal_info{}; - - if (path.path.points.empty()) { - return turn_signal_info; - } - - // desired start pose = prepare start pose - turn_signal_info.desired_start_point = std::invoke([&]() { - const auto blinker_start_duration = planner_data_->parameters.turn_signal_search_time; - const auto diff_time = path.info.duration.prepare - blinker_start_duration; - if (diff_time < 1e-5) { - return path.path.points.front().point.pose; - } - - const auto current_twist = getEgoTwist(); - const auto diff_length = std::abs(current_twist.linear.x) * diff_time; - return get_blinker_pose(path.path, diff_length); - }); - - // desired end pose - const auto length_ratio = - std::clamp(lane_change_parameters_->length_ratio_for_turn_signal_deactivation, 0.0, 1.0); - const auto desired_end_length = path.info.length.lane_changing * length_ratio; - turn_signal_info.desired_end_point = get_blinker_pose(shifted_path, desired_end_length); - - // required start pose = lane changing start pose - turn_signal_info.required_start_point = path.info.shift_line.start; - - // required end pose = in the middle of the lane change - const auto mid_lane_change_length = path.info.length.lane_changing / 2; - turn_signal_info.required_end_point = get_blinker_pose(shifted_path, mid_lane_change_length); - - return turn_signal_info; -} - bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const { const auto & route_handler = planner_data_->route_handler; diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 48a5a8722b0e9..bae733610905b 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -674,43 +674,6 @@ bool hasEnoughLengthToLaneChangeAfterAbort( return true; } -// TODO(Azu): In the future, get back lanelet within `to_back_dist` [m] from queried lane -lanelet::ConstLanelets getBackwardLanelets( - const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & current_pose, const double backward_length) -{ - if (target_lanes.empty()) { - return {}; - } - - const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); - - if (arc_length.length >= backward_length) { - return {}; - } - - const auto & front_lane = target_lanes.front(); - const auto preceding_lanes = route_handler.getPrecedingLaneletSequence( - front_lane, std::abs(backward_length - arc_length.length), {front_lane}); - - lanelet::ConstLanelets backward_lanes{}; - const auto num_of_lanes = std::invoke([&preceding_lanes]() { - size_t sum{0}; - for (const auto & lanes : preceding_lanes) { - sum += lanes.size(); - } - return sum; - }); - - backward_lanes.reserve(num_of_lanes); - - for (const auto & lanes : preceding_lanes) { - backward_lanes.insert(backward_lanes.end(), lanes.begin(), lanes.end()); - } - - return backward_lanes; -} - double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer) { return lateral_buffer + 0.5 * vehicle_width; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 5d40f2a8614ed..99e312f98f100 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -21,6 +21,8 @@ #include "behavior_path_planner_common/interface/steering_factor_interface.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include + #include #include #include @@ -215,6 +217,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node const std::shared_ptr & planner_data); std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index d2f58e7d7e017..e41061a2ffbd2 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -172,6 +172,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod } logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } std::vector BehaviorPathPlannerNode::getWaitingApprovalModules() @@ -238,6 +239,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.min_acc = declare_parameter("normal.min_acc"); p.max_acc = declare_parameter("normal.max_acc"); + p.max_vel = declare_parameter("max_vel"); p.backward_length_buffer_for_end_of_pull_over = declare_parameter("backward_length_buffer_for_end_of_pull_over"); p.backward_length_buffer_for_end_of_pull_out = @@ -413,6 +415,7 @@ void BehaviorPathPlannerNode::run() if (!path->points.empty()) { path_publisher_->publish(*path); + published_time_publisher_->publish_if_subscribed(path_publisher_, path->header.stamp); } else { RCLCPP_ERROR_THROTTLE( get_logger(), *get_clock(), 5000, "behavior path output is empty! Stop publish."); diff --git a/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md b/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md index c183e0627a674..bfc22bacc8ede 100644 --- a/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md +++ b/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md @@ -4,7 +4,7 @@ Turn Signal decider determines necessary blinkers. ## Purpose / Role -This module is responsible for activating a necessary blinker during driving. It uses rule-based algorithm to determine blinkers, and the details of this algorithm are described in the following sections. Note that this algorithm is strictly based on the Japanese Road Traffic Raw. +This module is responsible for activating a necessary blinker during driving. It uses rule-based algorithm to determine blinkers, and the details of this algorithm are described in the following sections. Note that this algorithm is strictly based on the Japanese Road Traffic Law. ### Assumptions @@ -16,7 +16,7 @@ Autoware has following order of priorities for turn signals. ### Limitations -Currently, this algorithm can sometimes give unnatural (not wrong) blinkers in a complicated situations. This is because it tries to follow the road traffic raw and cannot solve `blinker conflicts` clearly in that environment. +Currently, this algorithm can sometimes give unnatural (not wrong) blinkers in complicated situations. This is because it tries to follow the road traffic law and cannot solve `blinker conflicts` clearly in that environment. ## Parameters for turn signal decider @@ -59,16 +59,16 @@ For left turn, right turn, avoidance, lane change, goal planner and pull out, we Turn signal decider checks each lanelet on the map if it has `turn_direction` information. If a lanelet has this information, it activates necessary blinker based on this information. -- desired start point - The `search_distance` for blinkers at intersections is `v * turn_signal_search_time + turn_signal_intersection_search_distance`. Then the start point becomes `search_distance` meters before the start point of the intersection lanelet(depicted in gree in the following picture), where `v` is the velocity of the ego vehicle. However, if we set `turn_signal_distance` in the lanelet, we use that length as search distance. +- desired start point + The `search_distance` for blinkers at intersections is `v * turn_signal_search_time + turn_signal_intersection_search_distance`. Then the start point becomes `search_distance` meters before the start point of the intersection lanelet(depicted in green in the following picture), where `v` is the velocity of the ego vehicle. However, if we set `turn_signal_distance` in the lanelet, we use that length as search distance. -- desired end point +- desired end point Terminal point of the intersection lanelet. -- required start point +- required start point Initial point of the intersection lanelet. -- required end point +- required end point The earliest point that satisfies the following condition. $\theta - \theta_{\textrm{end}} < \delta$, where $\theta_{\textrm{end}}$ is yaw angle of the terminal point of the lanelet, $\theta$ is the angle of a required end point and $\delta$ is the threshold defined by the user. ![section_turn_signal](../images/turn_signal_decider/left_right_turn.drawio.svg) @@ -79,99 +79,99 @@ Avoidance can be separated into two sections, first section and second section. First section -- desired start point +- desired start point `v * turn_signal_search_time` meters before the start point of the avoidance shift path. -- desired end point +- desired end point Shift complete point where the path shift is completed. -- required start point +- required start point Avoidance start point. -- required end point +- required end point Shift complete point same as the desired end point. ![section_first_avoidance](../images/turn_signal_decider/avoidance_first_section.drawio.svg) Second section -- desired start point +- desired start point Shift complete point. -- desired end point +- desired end point Avoidance terminal point -- required start point +- required start point Shift complete point. -- required end point +- required end point Avoidance terminal point. ![section_second_avoidance](../images/turn_signal_decider/avoidance_second_section.drawio.svg) #### 3. Lane Change -- desired start point +- desired start point `v * turn_signal_search_time` meters before the start point of the lane change path. -- desired end point +- desired end point Terminal point of the lane change path. -- required start point +- required start point Initial point of the lane change path. -- required end point +- required end point Cross point with lane change path and boundary line of the adjacent lane. ![section_lane_change](../images/turn_signal_decider/lane_change.drawio.svg) #### 4. Pull out -- desired start point +- desired start point Start point of the path of pull out. -- desired end point +- desired end point Terminal point of the path of pull out. -- required start point +- required start point Start point of the path pull out. -- required end point +- required end point Terminal point of the path of pull out. ![section_pull_out](../images/turn_signal_decider/pull_out.drawio.svg) #### 5. Goal Planner -- desired start point +- desired start point `v * turn_signal_search_time` meters before the start point of the pull over path. -- desired end point +- desired end point Terminal point of the path of pull over. -- required start point +- required start point Start point of the path of pull over. -- required end point +- required end point Terminal point of the path of pull over. ![section_pull_over](../images/turn_signal_decider/pull_over.drawio.svg) When it comes to handle several blinkers, it gives priority to the first blinker that comes first. However, this rule sometimes activate unnatural blinkers, so turn signal decider uses the following five rules to decide the necessary turn signal. -- pattern1 +- pattern1 ![pattern1](../images/turn_signal_decider/pattern1.drawio.svg) -- pattern2 +- pattern2 ![pattern2](../images/turn_signal_decider/pattern2.drawio.svg) -- pattern3 +- pattern3 ![pattern3](../images/turn_signal_decider/pattern3.drawio.svg) -- pattern4 +- pattern4 ![pattern4](../images/turn_signal_decider/pattern4.drawio.svg) -- pattern5 +- pattern5 ![pattern5](../images/turn_signal_decider/pattern5.drawio.svg) Based on these five rules, turn signal decider can solve `blinker conflicts`. The following pictures show some examples of this kind of conflicts. diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp index 28943b5724fe8..d196f2bc551bb 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp @@ -21,6 +21,7 @@ #include "motion_utils/trajectory/trajectory.hpp" #include +#include #include #include #include @@ -165,6 +166,62 @@ struct PlannerData mutable std::vector drivable_area_expansion_prev_curvatures{}; mutable TurnSignalDecider turn_signal_decider; + std::pair getBehaviorTurnSignalInfo( + const PathWithLaneId & path, const size_t shift_start_idx, const size_t shift_end_idx, + const lanelet::ConstLanelets & current_lanelets, const double current_shift_length, + const bool is_driving_forward, const bool egos_lane_is_shifted, + const bool override_ego_stopped_check = false, const bool is_pull_out = false) const + { + if (shift_start_idx + 1 > path.points.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return std::make_pair(TurnSignalInfo{}, true); + } + + if (shift_end_idx + 1 > path.points.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return std::make_pair(TurnSignalInfo{}, true); + } + + std::vector lengths(path.points.size(), 0.0); + ShiftedPath shifted_path{path, lengths}; + ShiftLine shift_line; + + { + const auto start_pose = path.points.at(shift_start_idx).point.pose; + const auto start_shift_length = + lanelet::utils::getArcCoordinates(current_lanelets, start_pose).distance; + const auto end_pose = path.points.at(shift_end_idx).point.pose; + const auto end_shift_length = + lanelet::utils::getArcCoordinates(current_lanelets, end_pose).distance; + shifted_path.shift_length.at(shift_start_idx) = start_shift_length; + shifted_path.shift_length.at(shift_end_idx) = end_shift_length; + + shift_line.start = start_pose; + shift_line.end = end_pose; + shift_line.start_shift_length = start_shift_length; + shift_line.end_shift_length = end_shift_length; + shift_line.start_idx = shift_start_idx; + shift_line.end_idx = shift_end_idx; + } + + return turn_signal_decider.getBehaviorTurnSignalInfo( + shifted_path, shift_line, current_lanelets, route_handler, parameters, self_odometry, + current_shift_length, is_driving_forward, egos_lane_is_shifted, override_ego_stopped_check, + is_pull_out); + } + + std::pair getBehaviorTurnSignalInfo( + const ShiftedPath & path, const ShiftLine & shift_line, + const lanelet::ConstLanelets & current_lanelets, const double current_shift_length, + const bool is_driving_forward, const bool egos_lane_is_shifted, + const bool override_ego_stopped_check = false, const bool is_pull_out = false) const + { + return turn_signal_decider.getBehaviorTurnSignalInfo( + path, shift_line, current_lanelets, route_handler, parameters, self_odometry, + current_shift_length, is_driving_forward, egos_lane_is_shifted, override_ego_stopped_check, + is_pull_out); + } + TurnIndicatorsCommand getTurnSignal( const PathWithLaneId & path, const TurnSignalInfo & turn_signal_info, TurnSignalDebugData & debug_data) diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp index 811c89f61c066..1eae4a1c4c345 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp @@ -42,6 +42,7 @@ struct BehaviorPathPlannerParameters // common parameters double min_acc; double max_acc; + double max_vel; double minimum_pull_over_length; double minimum_pull_out_length; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp index cd639b5e81092..fe187567cd893 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp @@ -15,14 +15,24 @@ #ifndef BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ #define BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" + #include +#include +#include #include +#include +#include #include #include #include +#include + +#include #include +#include #include #include @@ -37,6 +47,7 @@ using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; +using nav_msgs::msg::Odometry; using route_handler::RouteHandler; const std::map g_signal_map = { @@ -100,6 +111,15 @@ class TurnSignalDecider std::pair getIntersectionTurnSignalFlag(); std::pair getIntersectionPoseAndDistance(); + std::pair getBehaviorTurnSignalInfo( + const ShiftedPath & path, const ShiftLine & shift_line, + const lanelet::ConstLanelets & current_lanelets, + const std::shared_ptr route_handler, + const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry, + const double current_shift_length, const bool is_driving_forward, + const bool egos_lane_is_shifted, const bool override_ego_stopped_check = false, + const bool is_pull_out = false) const; + private: std::optional getIntersectionTurnSignalInfo( const PathWithLaneId & path, const Pose & current_pose, const double current_vel, @@ -118,6 +138,132 @@ class TurnSignalDecider const double nearest_yaw_threshold); void initialize_intersection_info(); + inline bool isAvoidShift( + const double start_shift_length, const double end_shift_length, const double threshold) const + { + return std::abs(start_shift_length) < threshold && std::abs(end_shift_length) > threshold; + }; + + inline bool isReturnShift( + const double start_shift_length, const double end_shift_length, const double threshold) const + { + return std::abs(start_shift_length) > threshold && std::abs(end_shift_length) < threshold; + }; + + inline bool isLeftMiddleShift( + const double start_shift_length, const double end_shift_length, const double threshold) const + { + return start_shift_length > threshold && end_shift_length > threshold; + }; + + inline bool isRightMiddleShift( + const double start_shift_length, const double end_shift_length, const double threshold) const + { + return start_shift_length < threshold && end_shift_length < threshold; + }; + + inline bool existShiftSideLane( + const double start_shift_length, const double end_shift_length, const bool no_left_lanes, + const bool no_right_lanes, const double threshold) const + { + const auto relative_shift_length = end_shift_length - start_shift_length; + if (isAvoidShift(start_shift_length, end_shift_length, threshold)) { + // Left avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length > 0.0 && no_left_lanes) { + return false; + } + + // Right avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length < 0.0 && no_right_lanes) { + return false; + } + } + + if (isReturnShift(start_shift_length, end_shift_length, threshold)) { + // Right return. But there is no adjacent lane. No need blinker. + if (relative_shift_length > 0.0 && no_right_lanes) { + return false; + } + + // Left return. But there is no adjacent lane. No need blinker. + if (relative_shift_length < 0.0 && no_left_lanes) { + return false; + } + } + + if (isLeftMiddleShift(start_shift_length, end_shift_length, threshold)) { + // Left avoid. But there is no adjacent lane. No need blinker. + + if (relative_shift_length > 0.0 && no_left_lanes) { + return false; + } + + // Left return. But there is no adjacent lane. No need blinker. + if (relative_shift_length < 0.0 && no_left_lanes) { + return false; + } + } + + if (isRightMiddleShift(start_shift_length, end_shift_length, threshold)) { + // Right avoid. But there is no adjacent lane. No need blinker. + + if (relative_shift_length < 0.0 && no_right_lanes) { + return false; + } + + // Left avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length > 0.0 && no_right_lanes) { + return false; + } + } + return true; + }; + + inline bool straddleRoadBound( + const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes, + const vehicle_info_util::VehicleInfo & vehicle_info) const + { + using boost::geometry::intersects; + using tier4_autoware_utils::pose2transform; + using tier4_autoware_utils::transformVector; + + const auto footprint = vehicle_info.createFootprint(); + + for (const auto & lane : lanes) { + for (size_t i = shift_line.start_idx; i < shift_line.end_idx; ++i) { + const auto transform = pose2transform(path.path.points.at(i).point.pose); + const auto shifted_vehicle_footprint = transformVector(footprint, transform); + + if (intersects(lane.leftBound2d().basicLineString(), shifted_vehicle_footprint)) { + return true; + } + + if (intersects(lane.rightBound2d().basicLineString(), shifted_vehicle_footprint)) { + return true; + } + } + } + + return false; + }; + + inline bool isNearEndOfShift( + const double start_shift_length, const double end_shift_length, const Point & ego_pos, + const lanelet::ConstLanelets & original_lanes, const double threshold) const + { + using boost::geometry::within; + using lanelet::utils::to2D; + using lanelet::utils::conversion::toLaneletPoint; + + if (!isReturnShift(start_shift_length, end_shift_length, threshold)) { + return false; + } + + return std::any_of(original_lanes.begin(), original_lanes.end(), [&ego_pos](const auto & lane) { + return within(to2D(toLaneletPoint(ego_pos)), lane.polygon2d().basicPolygon()); + }); + }; + geometry_msgs::msg::Quaternion calc_orientation(const Point & src_point, const Point & dst_point); rclcpp::Logger logger_{ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp index dcee7696933dd..22db593b45586 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp @@ -299,6 +299,10 @@ lanelet::ConstLanelets extendPrevLane( lanelet::ConstLanelets extendLanes( const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes); +lanelet::ConstLanelets getBackwardLanelets( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & current_pose, const double backward_length); + lanelet::ConstLanelets getExtendedCurrentLanes( const std::shared_ptr & planner_data); diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp index 6ddc0df4eebf4..7bb2bab20bdcd 100644 --- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp @@ -33,6 +33,8 @@ namespace behavior_path_planner { +using motion_utils::calcSignedArcLength; + double calc_distance( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, const Pose & input_point, const double nearest_dist_threshold, const double nearest_yaw_threshold) @@ -43,6 +45,11 @@ double calc_distance( path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx); } +/*** + * @brief: + * Gets the turn signal info after comparing the turn signal info output from the behavior path + * module and comparing it to turn signal info obtained from intersections. + */ TurnIndicatorsCommand TurnSignalDecider::getTurnSignal( const std::shared_ptr & route_handler, const PathWithLaneId & path, const TurnSignalInfo & turn_signal_info, const Pose & current_pose, const double current_vel, @@ -606,4 +613,145 @@ geometry_msgs::msg::Quaternion TurnSignalDecider::calc_orientation( const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point); return tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); } + +std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( + const ShiftedPath & path, const ShiftLine & shift_line, + const lanelet::ConstLanelets & current_lanelets, + const std::shared_ptr route_handler, + const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry, + const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted, + const bool override_ego_stopped_check, const bool is_pull_out) const +{ + constexpr double THRESHOLD = 0.1; + const auto & p = parameters; + const auto & rh = route_handler; + const auto & ego_pose = self_odometry->pose.pose; + const auto & ego_speed = self_odometry->twist.twist.linear.x; + + if (!is_driving_forward) { + TurnSignalInfo turn_signal_info{}; + turn_signal_info.hazard_signal.command = HazardLightsCommand::ENABLE; + const auto back_start_pose = rh->getOriginalStartPose(); + const Pose & start_pose = self_odometry->pose.pose; + + turn_signal_info.desired_start_point = back_start_pose; + turn_signal_info.required_start_point = back_start_pose; + // pull_out start_pose is same to backward driving end_pose + turn_signal_info.required_end_point = start_pose; + turn_signal_info.desired_end_point = start_pose; + return std::make_pair(turn_signal_info, false); + } + + if (shift_line.start_idx + 1 > path.shift_length.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return std::make_pair(TurnSignalInfo{}, true); + } + + if (shift_line.start_idx + 1 > path.path.points.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return std::make_pair(TurnSignalInfo{}, true); + } + + if (shift_line.end_idx + 1 > path.shift_length.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return std::make_pair(TurnSignalInfo{}, true); + } + + if (shift_line.end_idx + 1 > path.path.points.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return std::make_pair(TurnSignalInfo{}, true); + } + + const auto [start_shift_length, end_shift_length] = + std::invoke([&path, &shift_line, &egos_lane_is_shifted]() -> std::pair { + const auto temp_start_shift_length = path.shift_length.at(shift_line.start_idx); + const auto temp_end_shift_length = path.shift_length.at(shift_line.end_idx); + // Shift is done using the target lane and not current ego's lane + if (!egos_lane_is_shifted) { + return std::make_pair(temp_end_shift_length, -temp_start_shift_length); + } + return std::make_pair(temp_start_shift_length, temp_end_shift_length); + }); + + const auto relative_shift_length = end_shift_length - start_shift_length; + + // If shift length is shorter than the threshold, it does not need to turn on blinkers + if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) { + return std::make_pair(TurnSignalInfo{}, true); + } + + // If the vehicle does not shift anymore, we turn off the blinker + if (std::fabs(end_shift_length - current_shift_length) < THRESHOLD) { + return std::make_pair(TurnSignalInfo{}, true); + } + + const auto get_command = [](const auto & shift_length) { + return shift_length > 0.0 ? TurnIndicatorsCommand::ENABLE_LEFT + : TurnIndicatorsCommand::ENABLE_RIGHT; + }; + + const auto signal_prepare_distance = + std::max(ego_speed * p.turn_signal_search_time, p.turn_signal_minimum_search_distance); + const auto ego_front_to_shift_start = + calcSignedArcLength(path.path.points, ego_pose.position, shift_line.start_idx) - + p.vehicle_info.max_longitudinal_offset_m; + + if (signal_prepare_distance < ego_front_to_shift_start) { + return std::make_pair(TurnSignalInfo{}, false); + } + + const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose; + const auto blinker_end_pose = path.path.points.at(shift_line.end_idx).point.pose; + const auto get_start_pose = [&](const auto & ego_to_shift_start) { + return ego_to_shift_start > 0.0 ? ego_pose : blinker_start_pose; + }; + + TurnSignalInfo turn_signal_info{}; + turn_signal_info.desired_start_point = get_start_pose(ego_front_to_shift_start); + turn_signal_info.desired_end_point = blinker_end_pose; + turn_signal_info.required_start_point = blinker_start_pose; + turn_signal_info.required_end_point = blinker_end_pose; + turn_signal_info.turn_signal.command = get_command(relative_shift_length); + + if (!p.turn_signal_on_swerving) { + return std::make_pair(turn_signal_info, false); + } + + lanelet::ConstLanelet lanelet; + const auto query_pose = (egos_lane_is_shifted) ? shift_line.end : shift_line.start; + if (!rh->getClosestLaneletWithinRoute(query_pose, &lanelet)) { + return std::make_pair(TurnSignalInfo{}, true); + } + + const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true); + const auto left_opposite_lanes = rh->getLeftOppositeLanelets(lanelet); + const auto right_same_direction_lane = rh->getRightLanelet(lanelet, true, true); + const auto right_opposite_lanes = rh->getRightOppositeLanelets(lanelet); + const auto has_left_lane = left_same_direction_lane.has_value() || !left_opposite_lanes.empty(); + const auto has_right_lane = + right_same_direction_lane.has_value() || !right_opposite_lanes.empty(); + + if ( + !is_pull_out && !existShiftSideLane( + start_shift_length, end_shift_length, !has_left_lane, !has_right_lane, + p.turn_signal_shift_length_threshold)) { + return std::make_pair(TurnSignalInfo{}, true); + } + + if (!straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) { + return std::make_pair(TurnSignalInfo{}, true); + } + + constexpr double STOPPED_THRESHOLD = 0.1; // [m/s] + if (ego_speed < STOPPED_THRESHOLD && !override_ego_stopped_check) { + if (isNearEndOfShift( + start_shift_length, end_shift_length, ego_pose.position, current_lanelets, + p.turn_signal_shift_length_threshold)) { + return std::make_pair(TurnSignalInfo{}, true); + } + } + + return std::make_pair(turn_signal_info, false); +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 6154d29855f7a..571a40859202f 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -1481,6 +1481,10 @@ std::vector postProcess( const auto & vehicle_length = planner_data->parameters.vehicle_length; constexpr double overlap_threshold = 0.01; + if (original_bound.size() < 2) { + return original_bound; + } + const auto addPoints = [](const lanelet::ConstLineString3d & points, std::vector & bound) { for (const auto & bound_p : points) { diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 188c50993e4ae..ea9872f268678 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -78,7 +78,7 @@ PredictedObjects filterObjects( const std::shared_ptr & params) { // Guard - if (objects->objects.empty()) { + if (objects->objects.empty() || !params) { return PredictedObjects(); } diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp index 308cf9c4fed2c..6fcfad2209815 100644 --- a/planning/behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/utils.cpp @@ -1514,6 +1514,43 @@ lanelet::ConstLanelets getExtendedCurrentLanesFromPath( return lanes; } +// TODO(Azu): In the future, get back lanelet within `to_back_dist` [m] from queried lane +lanelet::ConstLanelets getBackwardLanelets( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & current_pose, const double backward_length) +{ + if (target_lanes.empty()) { + return {}; + } + + const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); + + if (arc_length.length >= backward_length) { + return {}; + } + + const auto & front_lane = target_lanes.front(); + const auto preceding_lanes = route_handler.getPrecedingLaneletSequence( + front_lane, std::abs(backward_length - arc_length.length), {front_lane}); + + lanelet::ConstLanelets backward_lanes{}; + const auto num_of_lanes = std::invoke([&preceding_lanes]() { + size_t sum{0}; + for (const auto & lanes : preceding_lanes) { + sum += lanes.size(); + } + return sum; + }); + + backward_lanes.reserve(num_of_lanes); + + for (const auto & lanes : preceding_lanes) { + backward_lanes.insert(backward_lanes.end(), lanes.begin(), lanes.end()); + } + + return backward_lanes; +} + lanelet::ConstLanelets calcLaneAroundPose( const std::shared_ptr route_handler, const Pose & pose, const double forward_length, const double backward_length, const double dist_threshold, const double yaw_threshold) diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 4b52aae91d032..ff0550e4d4867 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -222,9 +222,9 @@ This ordering is beneficial when the priority is to minimize the backward distan - **Applying RSS in Dynamic Collision Detection**: Collision detection is based on the RSS (Responsibility-Sensitive Safety) model to evaluate if a safe distance is maintained. See [safety check feature explanation](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) -- **Collision check performed range**: Safety checks for collisions with dynamic objects are conducted within the defined boundaries between the start and end points of each maneuver, ensuring there is no overlap with the road lane's centerline. This is to avoid hindering the progress of following vehicles. +- **Collision check performed range**: Safety checks for collisions with dynamic objects are conducted within the defined boundaries between the start and end points of each maneuver, ensuring the ego vehicle does not impede or hinder the progress of dynamic objects that come from behind it. -- **Collision response policy**: Should a collision with dynamic objects be detected along the generated path, deactivate module decision is registered if collision detection occurs before departure. If the vehicle has already commenced movement, an attempt to stop will be made, provided it's feasible within the braking constraints and without crossing the travel lane's centerline. +- **Collision response policy**: Should a collision with dynamic objects be detected along the generated path, deactivate module decision is registered if collision detection occurs before departure. If the vehicle has already commenced movement, an attempt to stop will be made, provided it's feasible within the braking constraints and that the rear vehicle can pass through the gap between the ego vehicle and the lane border. ```plantuml @startuml @@ -235,7 +235,7 @@ if (Collision with dynamic objects detected?) then (yes) if (Before departure?) then (yes) :Deactivate module decision is registered; else (no) - if (Can stop within constraints \n && \n no crossing centerline?) then (yes) + if (Can stop within constraints \n && \n Has sufficient space for rear vehicle to drive?) then (yes) :Stop; else (no) :Continue with caution; @@ -250,7 +250,7 @@ stop #### **example of safety check performed range for shift pull out** -Give an example of safety verification range for shift pull out. The safety check is performed from the start of the shift to the end of the shift. And if the vehicle footprint overlaps with the center line of the road lane, the safety check against dynamic objects is disabled. +Give an example of safety verification range for shift pull out. The safety check is performed from the start of the shift to the end of the shift. And if the vehicle footprint does not leave enough space for a rear vehicle to drive through, the safety check against dynamic objects is disabled.
![safety check performed range for shift pull out](images/collision_check_range_shift_pull_out.drawio.svg){width=1100} @@ -319,27 +319,28 @@ PullOutPath --o PullOutPlannerBase ## General parameters for start_planner -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------------- | -| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | -| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | -| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | -| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | -| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | -| collision_check_margins | [m] | double | Obstacle collision check margins list | [2.0, 1.0, 0.5, 0.1] | -| shift_collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | -10.0 | -| geometric_collision_check_distance_from_end | [m] | double | collision check distance from end geometric end pose | 0.0 | -| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | -| object_types_to_check_for_path_generation.check_car | - | bool | flag to check car for path generation | true | -| object_types_to_check_for_path_generation.check_truck | - | bool | flag to check truck for path generation | true | -| object_types_to_check_for_path_generation.check_bus | - | bool | flag to check bus for path generation | true | -| object_types_to_check_for_path_generation.check_bicycle | - | bool | flag to check bicycle for path generation | true | -| object_types_to_check_for_path_generation.check_motorcycle | - | bool | flag to check motorcycle for path generation | true | -| object_types_to_check_for_path_generation.check_pedestrian | - | bool | flag to check pedestrian for path generation | true | -| object_types_to_check_for_path_generation.check_unknown | - | bool | flag to check unknown for path generation | true | -| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------------------- | :---- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------------- | +| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | +| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | +| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | +| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | +| collision_check_margins | [m] | double | Obstacle collision check margins list | [2.0, 1.0, 0.5, 0.1] | +| shift_collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | -10.0 | +| geometric_collision_check_distance_from_end | [m] | double | collision check distance from end geometric end pose | 0.0 | +| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | +| extra_width_margin_for_rear_obstacle | [m] | double | extra width that is added to the perceived rear obstacle's width when deciding if the rear obstacle can overtake the ego vehicle while it is merging to a lane from the shoulder lane. | 0.5 | +| object_types_to_check_for_path_generation.check_car | - | bool | flag to check car for path generation | true | +| object_types_to_check_for_path_generation.check_truck | - | bool | flag to check truck for path generation | true | +| object_types_to_check_for_path_generation.check_bus | - | bool | flag to check bus for path generation | true | +| object_types_to_check_for_path_generation.check_bicycle | - | bool | flag to check bicycle for path generation | true | +| object_types_to_check_for_path_generation.check_motorcycle | - | bool | flag to check motorcycle for path generation | true | +| object_types_to_check_for_path_generation.check_pedestrian | - | bool | flag to check pedestrian for path generation | true | +| object_types_to_check_for_path_generation.check_unknown | - | bool | flag to check unknown for path generation | true | +| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | ### **Ego vehicle's velocity planning** diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index fad29b84c9c76..b17db7f907471 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -7,6 +7,7 @@ th_stopped_time: 1.0 collision_check_margins: [2.0, 1.0, 0.5, 0.1] collision_check_margin_from_front_object: 5.0 + extra_width_margin_for_rear_obstacle: 0.5 th_moving_object_velocity: 1.0 object_types_to_check_for_path_generation: check_car: true diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 07c81d2edd050..1e32237ffd870 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -64,6 +64,7 @@ struct StartPlannerParameters double th_distance_to_middle_of_the_road{0.0}; double intersection_search_length{0.0}; double length_ratio_for_turn_signal_deactivation_near_intersection{0.0}; + double extra_width_margin_for_rear_obstacle{0.0}; std::vector collision_check_margins{}; double collision_check_margin_from_front_object{0.0}; double th_moving_object_velocity{0.0}; diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp index f38279d172009..75e55d6eab410 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp @@ -38,7 +38,7 @@ class FreespacePullOut : public PullOutPlannerBase rclcpp::Node & node, const StartPlannerParameters & parameters, const vehicle_info_util::VehicleInfo & vehicle_info); - PlannerType getPlannerType() override { return PlannerType::FREESPACE; } + PlannerType getPlannerType() const override { return PlannerType::FREESPACE; } std::optional plan(const Pose & start_pose, const Pose & end_pose) override; diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp index 68a5d8100a271..138aaaddd6981 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp @@ -34,7 +34,7 @@ class GeometricPullOut : public PullOutPlannerBase rclcpp::Node & node, const StartPlannerParameters & parameters, const std::shared_ptr lane_departure_checker); - PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; }; + PlannerType getPlannerType() const override { return PlannerType::GEOMETRIC; }; std::optional plan(const Pose & start_pose, const Pose & goal_pose) override; GeometricParallelParking planner_; diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp index 0144fdd45ae50..0126ab47d1ae6 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -16,8 +16,10 @@ #define BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ #include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_start_planner_module/data_structs.hpp" #include "behavior_path_start_planner_module/pull_out_path.hpp" +#include "behavior_path_start_planner_module/util.hpp" #include #include @@ -54,14 +56,47 @@ class PullOutPlannerBase planner_data_ = planner_data; } - virtual PlannerType getPlannerType() = 0; + void setCollisionCheckMargin(const double collision_check_margin) + { + collision_check_margin_ = collision_check_margin; + }; + virtual PlannerType getPlannerType() const = 0; virtual std::optional plan(const Pose & start_pose, const Pose & goal_pose) = 0; protected: + bool isPullOutPathCollided( + behavior_path_planner::PullOutPath & pull_out_path, + double collision_check_distance_from_end) const + { + // check for collisions + const auto & dynamic_objects = planner_data_->dynamic_object; + const auto pull_out_lanes = start_planner_utils::getPullOutLanes( + planner_data_, + planner_data_->parameters.backward_path_length + parameters_.max_back_distance); + const auto & vehicle_footprint = vehicle_info_.createFootprint(); + // extract stop objects in pull out lane for collision check + const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + *dynamic_objects, parameters_.th_moving_object_velocity); + auto [pull_out_lane_stop_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); + utils::path_safety_checker::filterObjectsByClass( + pull_out_lane_stop_objects, parameters_.object_types_to_check_for_path_generation); + + const auto collision_check_section_path = + behavior_path_planner::start_planner_utils::extractCollisionCheckSection( + pull_out_path, collision_check_distance_from_end); + if (!collision_check_section_path) return true; + + return utils::checkCollisionBetweenPathFootprintsAndObjects( + vehicle_footprint_, collision_check_section_path.value(), pull_out_lane_stop_objects, + collision_check_margin_); + }; std::shared_ptr planner_data_; vehicle_info_util::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; StartPlannerParameters parameters_; + double collision_check_margin_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp index 2d50ae189dc13..f831eeda9778d 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp @@ -36,7 +36,7 @@ class ShiftPullOut : public PullOutPlannerBase rclcpp::Node & node, const StartPlannerParameters & parameters, std::shared_ptr & lane_departure_checker); - PlannerType getPlannerType() override { return PlannerType::SHIFT; }; + PlannerType getPlannerType() const override { return PlannerType::SHIFT; }; std::optional plan(const Pose & start_pose, const Pose & goal_pose) override; std::vector calcPullOutPaths( diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 1368124929367..a30d7c379f80c 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -33,6 +33,7 @@ #include +#include #include #include @@ -70,8 +71,9 @@ struct PullOutStatus bool prev_is_safe_dynamic_objects{false}; std::shared_ptr prev_stop_path_after_approval{nullptr}; std::optional stop_pose{std::nullopt}; - //! record the first time when the state changed from !isActivated() to isActivated() - std::optional first_engaged_time{std::nullopt}; + //! record the first time when ego started forward-driving (maybe after backward driving + //! completion) in AUTONOMOUS operation mode + std::optional first_engaged_and_driving_forward_time{std::nullopt}; PullOutStatus() {} }; @@ -207,7 +209,8 @@ class StartPlannerModule : public SceneModuleInterface bool isModuleRunning() const; bool isCurrentPoseOnMiddleOfTheRoad() const; - bool isOverlapWithCenterLane() const; + bool isPreventingRearVehicleFromPassingThrough() const; + bool isCloseToOriginalStartPose() const; bool hasArrivedAtGoal() const; bool isMoving() const; @@ -238,6 +241,10 @@ class StartPlannerModule : public SceneModuleInterface PullOutStatus status_; mutable StartPlannerDebugData debug_data_; + // Keeps track of lanelets that should be ignored when calculating the turnSignalInfo for this + // module's output. If the ego vehicle is in this lanelet, the calculation is skipped. + std::optional ignore_signal_{std::nullopt}; + std::deque odometry_buffer_; std::unique_ptr last_route_received_time_; @@ -262,7 +269,7 @@ class StartPlannerModule : public SceneModuleInterface std::shared_ptr lane_departure_checker_; // turn signal - TurnSignalInfo calcTurnSignalInfo() const; + TurnSignalInfo calcTurnSignalInfo(); void incrementPathIndex(); PathWithLaneId getCurrentPath() const; @@ -278,7 +285,7 @@ class StartPlannerModule : public SceneModuleInterface const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_pose, const double velocity_threshold, const double object_check_backward_distance, const double object_check_forward_distance) const; - bool needToPrepareBlinkerBeforeStart() const; + bool needToPrepareBlinkerBeforeStartDrivingForward() const; bool hasFinishedPullOut() const; bool hasFinishedBackwardDriving() const; bool hasCollisionWithDynamicObjects() const; diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp index 147632329d926..5d55ce1519217 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp @@ -19,6 +19,7 @@ #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "behavior_path_start_planner_module/pull_out_path.hpp" #include @@ -50,6 +51,8 @@ lanelet::ConstLanelets getPullOutLanes( const std::shared_ptr & planner_data, const double backward_length); Pose getBackedPose( const Pose & current_pose, const double & yaw_shoulder_lane, const double & back_distance); +std::optional extractCollisionCheckSection( + const PullOutPath & path, const double collision_check_distance_from_end); } // namespace behavior_path_planner::start_planner_utils #endif // BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ diff --git a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp index a5aeb5b257d39..0f7587fdff476 100644 --- a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -114,6 +114,10 @@ std::optional GeometricPullOut::plan(const Pose & start_pose, const output.start_pose = planner_.getArcPaths().at(0).points.front().point.pose; output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose; + if (isPullOutPathCollided(output, parameters_.geometric_collision_check_distance_from_end)) { + return {}; + } + return output; } } // namespace behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index beeb675efd3ae..050d591128a15 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -44,6 +44,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.intersection_search_length = node->declare_parameter(ns + "intersection_search_length"); p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter( ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); + p.extra_width_margin_for_rear_obstacle = + node->declare_parameter(ns + "extra_width_margin_for_rear_obstacle"); p.collision_check_margins = node->declare_parameter>(ns + "collision_check_margins"); p.collision_check_margin_from_front_object = @@ -371,6 +373,10 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "length_ratio_for_turn_signal_deactivation_near_intersection", p->length_ratio_for_turn_signal_deactivation_near_intersection); + updateParam( + parameters, ns + "extra_width_margin_for_rear_obstacle", + p->extra_width_margin_for_rear_obstacle); + updateParam>( parameters, ns + "collision_check_margins", p->collision_check_margins); updateParam( diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index f5674cfb288d0..c6a56139e63d2 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -110,10 +110,14 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos const auto cropped_path = lane_departure_checker_->cropPointsOutsideOfLanes( lanelet_map_ptr, shift_path, start_segment_idx); - + if (cropped_path.points.empty()) continue; shift_path.points = cropped_path.points; shift_path.header = planner_data_->route_handler->getRouteHeader(); + if (isPullOutPathCollided(pull_out_path, parameters_.shift_collision_check_distance_from_end)) { + continue; + } + return pull_out_path; } diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 3c08d62f500ae..3482618d9319c 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -39,6 +39,7 @@ #include using behavior_path_planner::utils::parking_departure::initializeCollisionCheckDebugMap; +using motion_utils::calcLateralOffset; using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::calcOffsetPose; @@ -118,7 +119,7 @@ void StartPlannerModule::onFreespacePlannerTimer() BehaviorModuleOutput StartPlannerModule::run() { updateData(); - if (!isActivated() || needToPrepareBlinkerBeforeStart()) { + if (!isActivated() || needToPrepareBlinkerBeforeStartDrivingForward()) { return planWaitingApproval(); } @@ -178,8 +179,8 @@ void StartPlannerModule::updateData() if ( planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS && - !status_.first_engaged_time) { - status_.first_engaged_time = clock_->now(); + status_.driving_forward && !status_.first_engaged_and_driving_forward_time) { + status_.first_engaged_and_driving_forward_time = clock_->now(); } status_.backward_driving_complete = hasFinishedBackwardDriving(); @@ -220,7 +221,7 @@ bool StartPlannerModule::receivedNewRoute() const bool StartPlannerModule::requiresDynamicObjectsCollisionDetection() const { return parameters_->safety_check_params.enable_safety_check && status_.driving_forward && - !isOverlapWithCenterLane(); + !isPreventingRearVehicleFromPassingThrough(); } bool StartPlannerModule::noMovingObjectsAround() const @@ -287,35 +288,140 @@ bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const return std::abs(lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road; } -bool StartPlannerModule::isOverlapWithCenterLane() const +bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const { - const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const auto current_lanes = utils::getCurrentLanes(planner_data_); - const auto local_vehicle_footprint = vehicle_info_.createFootprint(); - const auto vehicle_footprint = - transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(current_pose)); - for (const auto & current_lane : current_lanes) { - std::vector centerline_points; - for (const auto & point : current_lane.centerline()) { - geometry_msgs::msg::Point center_point = lanelet::utils::conversion::toGeomMsgPt(point); - centerline_points.push_back(center_point); - } + const auto & current_pose = planner_data_->self_odometry->pose.pose; + const auto & dynamic_object = planner_data_->dynamic_object; + const auto & route_handler = planner_data_->route_handler; + const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); - for (size_t i = 0; i < centerline_points.size() - 1; ++i) { - const auto & p1 = centerline_points.at(i); - const auto & p2 = centerline_points.at(i + 1); - for (size_t j = 0; j < vehicle_footprint.size() - 1; ++j) { - const auto p3 = tier4_autoware_utils::toMsg(vehicle_footprint[j].to_3d()); - const auto p4 = tier4_autoware_utils::toMsg(vehicle_footprint[j + 1].to_3d()); - const auto intersection = tier4_autoware_utils::intersect(p1, p2, p3, p4); - - if (intersection.has_value()) { - return true; - } + const auto target_lanes = utils::getCurrentLanes(planner_data_); + if (target_lanes.empty()) return false; + + // Define functions to get distance between a point and a lane's boundaries. + auto calc_absolute_lateral_offset = [&]( + const lanelet::ConstLineString2d & boundary_line, + const geometry_msgs::msg::Pose & search_pose) { + std::vector boundary_path; + std::for_each( + boundary_line.begin(), boundary_line.end(), [&boundary_path](const auto & boundary_point) { + const double x = boundary_point.x(); + const double y = boundary_point.y(); + boundary_path.push_back(tier4_autoware_utils::createPoint(x, y, 0.0)); + }); + + return std::fabs(calcLateralOffset(boundary_path, search_pose.position)); + }; + + // Check from what side of the road the ego is merging + const auto centerline_path = + route_handler->getCenterLinePath(target_lanes, 0.0, std::numeric_limits::max()); + const auto start_pose_nearest_segment_index = + motion_utils::findNearestSegmentIndex(centerline_path.points, start_pose); + if (!start_pose_nearest_segment_index) return false; + + const auto start_pose_point_msg = tier4_autoware_utils::createPoint( + start_pose.position.x, start_pose.position.y, start_pose.position.z); + const auto starting_pose_lateral_offset = motion_utils::calcLateralOffset( + centerline_path.points, start_pose_point_msg, start_pose_nearest_segment_index.value()); + if (std::isnan(starting_pose_lateral_offset)) return false; + + RCLCPP_DEBUG(getLogger(), "starting pose lateral offset: %f", starting_pose_lateral_offset); + const bool ego_is_merging_from_the_left = (starting_pose_lateral_offset > 0.0); + + // Get the ego's overhang point closest to the centerline path and the gap between said point and + // the lane's border. + auto get_gap_between_ego_and_lane_border = + [&]( + geometry_msgs::msg::Pose & ego_overhang_point_as_pose, + const bool ego_is_merging_from_the_left) -> std::optional { + const auto local_vehicle_footprint = vehicle_info_.createFootprint(); + const auto vehicle_footprint = + transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(current_pose)); + double smallest_lateral_gap_between_ego_and_border = std::numeric_limits::max(); + for (const auto & point : vehicle_footprint) { + geometry_msgs::msg::Pose point_pose; + point_pose.position.x = point.x(); + point_pose.position.y = point.y(); + point_pose.position.z = 0.0; + + lanelet::Lanelet closest_lanelet; + lanelet::utils::query::getClosestLanelet(target_lanes, point_pose, &closest_lanelet); + lanelet::ConstLanelet closest_lanelet_const(closest_lanelet.constData()); + + const lanelet::ConstLineString2d current_lane_bound = (ego_is_merging_from_the_left) + ? closest_lanelet_const.rightBound2d() + : closest_lanelet_const.leftBound2d(); + const double current_point_lateral_gap = + calc_absolute_lateral_offset(current_lane_bound, point_pose); + if (current_point_lateral_gap < smallest_lateral_gap_between_ego_and_border) { + smallest_lateral_gap_between_ego_and_border = current_point_lateral_gap; + ego_overhang_point_as_pose.position.x = point.x(); + ego_overhang_point_as_pose.position.y = point.y(); + ego_overhang_point_as_pose.position.z = 0.0; } } - } - return false; + if (smallest_lateral_gap_between_ego_and_border == std::numeric_limits::max()) { + return std::nullopt; + } + return smallest_lateral_gap_between_ego_and_border; + }; + + geometry_msgs::msg::Pose ego_overhang_point_as_pose; + const auto gap_between_ego_and_lane_border = + get_gap_between_ego_and_lane_border(ego_overhang_point_as_pose, ego_is_merging_from_the_left); + if (!gap_between_ego_and_lane_border) return false; + + // Get the lanelets that will be queried for target objects + const auto relevant_lanelets = std::invoke([&]() -> std::optional { + lanelet::Lanelet closest_lanelet; + const bool is_closest_lanelet = lanelet::utils::query::getClosestLanelet( + target_lanes, ego_overhang_point_as_pose, &closest_lanelet); + if (!is_closest_lanelet) return std::nullopt; + lanelet::ConstLanelet closest_lanelet_const(closest_lanelet.constData()); + // Check backwards just in case the Vehicle behind ego is in a different lanelet + constexpr double backwards_length = 200.0; + const auto prev_lanes = behavior_path_planner::utils::getBackwardLanelets( + *route_handler, target_lanes, current_pose, backwards_length); + // return all the relevant lanelets + lanelet::ConstLanelets relevant_lanelets{closest_lanelet_const}; + relevant_lanelets.insert(relevant_lanelets.end(), prev_lanes.begin(), prev_lanes.end()); + return relevant_lanelets; + }); + if (!relevant_lanelets) return false; + + // filtering objects with velocity, position and class + const auto filtered_objects = utils::path_safety_checker::filterObjects( + dynamic_object, route_handler, relevant_lanelets.value(), current_pose.position, + objects_filtering_params_); + if (filtered_objects.objects.empty()) return false; + + // filtering objects based on the current position's lane + const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + relevant_lanelets.value(), route_handler, filtered_objects, objects_filtering_params_); + if (target_objects_on_lane.on_current_lane.empty()) return false; + + // Get the closest target obj width in the relevant lanes + const auto closest_object_width = std::invoke([&]() -> std::optional { + double arc_length_to_closet_object = std::numeric_limits::max(); + double closest_object_width = -1.0; + std::for_each( + target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(), + [&](const auto & o) { + const auto arc_length = motion_utils::calcSignedArcLength( + centerline_path.points, current_pose.position, o.initial_pose.pose.position); + if (arc_length > 0.0) return; + if (std::abs(arc_length) >= std::abs(arc_length_to_closet_object)) return; + arc_length_to_closet_object = arc_length; + closest_object_width = o.shape.dimensions.y; + }); + if (closest_object_width < 0.0) return std::nullopt; + return closest_object_width; + }); + if (!closest_object_width) return false; + // Decide if the closest object does not fit in the gap left by the ego vehicle. + return closest_object_width.value() + parameters_->extra_width_margin_for_rear_obstacle > + gap_between_ego_and_lane_border.value(); } bool StartPlannerModule::isCloseToOriginalStartPose() const @@ -661,22 +767,11 @@ bool StartPlannerModule::findPullOutPath( const Pose & start_pose_candidate, const std::shared_ptr & planner, const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin) { - const auto & dynamic_objects = planner_data_->dynamic_object; - const auto pull_out_lanes = start_planner_utils::getPullOutLanes( - planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); - const auto & vehicle_footprint = vehicle_info_.createFootprint(); - // extract stop objects in pull out lane for collision check - const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - *dynamic_objects, parameters_->th_moving_object_velocity); - auto [pull_out_lane_stop_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); - utils::path_safety_checker::filterObjectsByClass( - pull_out_lane_stop_objects, parameters_->object_types_to_check_for_path_generation); - // if start_pose_candidate is far from refined_start_pose, backward driving is necessary const bool backward_is_unnecessary = tier4_autoware_utils::calcDistance2d(start_pose_candidate, refined_start_pose) < 0.01; + planner->setCollisionCheckMargin(collision_check_margin); planner->setPlannerData(planner_data_); const auto pull_out_path = planner->plan(start_pose_candidate, goal_pose); @@ -685,13 +780,6 @@ bool StartPlannerModule::findPullOutPath( return false; } - // check collision - if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint, extractCollisionCheckSection(*pull_out_path, planner->getPlannerType()), - pull_out_lane_stop_objects, collision_check_margin)) { - return false; - } - if (backward_is_unnecessary) { updateStatusWithCurrentPath(*pull_out_path, start_pose_candidate, planner->getPlannerType()); return true; @@ -702,49 +790,6 @@ bool StartPlannerModule::findPullOutPath( return true; } -PathWithLaneId StartPlannerModule::extractCollisionCheckSection( - const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type) -{ - const std::map collision_check_distances = { - {behavior_path_planner::PlannerType::SHIFT, - parameters_->shift_collision_check_distance_from_end}, - {behavior_path_planner::PlannerType::GEOMETRIC, - parameters_->geometric_collision_check_distance_from_end}}; - - const double collision_check_distance_from_end = collision_check_distances.at(planner_type); - - PathWithLaneId full_path; - for (const auto & partial_path : path.partial_paths) { - full_path.points.insert( - full_path.points.end(), partial_path.points.begin(), partial_path.points.end()); - } - - // Find the start index for collision check section based on the shift start pose - const auto shift_start_idx = - motion_utils::findNearestIndex(full_path.points, path.start_pose.position); - - // Find the end index for collision check section based on the end pose and collision check - // distance - const auto collision_check_end_idx = [&]() -> size_t { - const auto end_pose_offset = motion_utils::calcLongitudinalOffsetPose( - full_path.points, path.end_pose.position, collision_check_distance_from_end); - - return end_pose_offset - ? motion_utils::findNearestIndex(full_path.points, end_pose_offset->position) - : full_path.points.size() - 1; // Use the last point if offset pose is not calculable - }(); - - // Extract the collision check section from the full path - PathWithLaneId collision_check_section; - if (shift_start_idx < collision_check_end_idx) { - collision_check_section.points.assign( - full_path.points.begin() + shift_start_idx, - full_path.points.begin() + collision_check_end_idx + 1); - } - - return collision_check_section; -} - void StartPlannerModule::updateStatusWithCurrentPath( const behavior_path_planner::PullOutPath & path, const Pose & start_pose, const behavior_path_planner::PlannerType & planner_type) @@ -1073,13 +1118,15 @@ bool StartPlannerModule::hasFinishedPullOut() const return has_finished; } -bool StartPlannerModule::needToPrepareBlinkerBeforeStart() const +bool StartPlannerModule::needToPrepareBlinkerBeforeStartDrivingForward() const { - if (!status_.first_engaged_time) { - return true; + if (!status_.first_engaged_and_driving_forward_time) { + return false; } - const auto first_engaged_time = status_.first_engaged_time.value(); - const double elapsed = rclcpp::Duration(clock_->now() - first_engaged_time).seconds(); + const auto first_engaged_and_driving_forward_time = + status_.first_engaged_and_driving_forward_time.value(); + const double elapsed = + rclcpp::Duration(clock_->now() - first_engaged_and_driving_forward_time).seconds(); return elapsed < parameters_->prepare_time_before_start; } @@ -1107,103 +1154,70 @@ bool StartPlannerModule::hasFinishedCurrentPath() return is_near_target && isStopped(); } -TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const +TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() { - TurnSignalInfo turn_signal{}; // output + const auto path = getFullPath(); + if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info; const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const Pose & start_pose = status_.pull_out_path.start_pose; - const Pose & end_pose = status_.pull_out_path.end_pose; - - // turn on hazard light when backward driving - if (!status_.driving_forward) { - turn_signal.hazard_signal.command = HazardLightsCommand::ENABLE; - const auto back_start_pose = planner_data_->route_handler->getOriginalStartPose(); - turn_signal.desired_start_point = back_start_pose; - turn_signal.required_start_point = back_start_pose; - // pull_out start_pose is same to backward driving end_pose - turn_signal.required_end_point = start_pose; - turn_signal.desired_end_point = start_pose; - return turn_signal; - } - - // turn on right signal until passing pull_out end point - const auto path = getFullPath(); - // pull out path does not overlap - const double distance_from_end = - motion_utils::calcSignedArcLength(path.points, end_pose.position, current_pose.position); + const auto shift_start_idx = + motion_utils::findNearestIndex(path.points, status_.pull_out_path.start_pose.position); + const auto shift_end_idx = + motion_utils::findNearestIndex(path.points, status_.pull_out_path.end_pose.position); + const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + + const auto is_ignore_signal = [this](const lanelet::Id & id) { + if (!ignore_signal_.has_value()) { + return false; + } + return ignore_signal_.value() == id; + }; + + const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) { + return is_ignore ? std::make_optional(id) : std::nullopt; + }; - if (path.points.empty()) { - return {}; + lanelet::Lanelet closest_lanelet; + lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, &closest_lanelet); + + if (is_ignore_signal(closest_lanelet.id())) { + return getPreviousModuleOutput().turn_signal_info; } - // calculate lateral offset from pull out target lane center line - lanelet::ConstLanelet closest_road_lane; - const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_->max_back_distance; - const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), - /*forward_only_in_route*/ true); - lanelet::utils::query::getClosestLanelet(road_lanes, start_pose, &closest_road_lane); - const double lateral_offset = - lanelet::utils::getLateralDistanceToCenterline(closest_road_lane, start_pose); - - if (distance_from_end < 0.0 && lateral_offset > parameters_->th_turn_signal_on_lateral_offset) { - turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - } else if ( - distance_from_end < 0.0 && lateral_offset < -parameters_->th_turn_signal_on_lateral_offset) { - turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - } else { - turn_signal.turn_signal.command = TurnIndicatorsCommand::DISABLE; - } - - turn_signal.desired_start_point = start_pose; - turn_signal.required_start_point = start_pose; - turn_signal.desired_end_point = end_pose; - - // check if intersection exists within search length - const bool is_near_intersection = std::invoke([&]() { - const double check_length = parameters_->intersection_search_length; - double accumulated_length = 0.0; - const size_t current_idx = motion_utils::findNearestIndex(path.points, current_pose.position); - for (size_t i = current_idx; i < path.points.size() - 1; ++i) { - const auto & p = path.points.at(i); - for (const auto & lane : planner_data_->route_handler->getLaneletsFromIds(p.lane_ids)) { - const std::string turn_direction = lane.attributeOr("turn_direction", "else"); - if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { - return true; - } - } - accumulated_length += tier4_autoware_utils::calcDistance2d(p, path.points.at(i + 1)); - if (accumulated_length > check_length) { - return false; - } - } - return false; - }); + const double current_shift_length = + lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance; - turn_signal.required_end_point = std::invoke([&]() { - if (is_near_intersection) return end_pose; - const double length_start_to_end = - motion_utils::calcSignedArcLength(path.points, start_pose.position, end_pose.position); - const auto ratio = std::clamp( - parameters_->length_ratio_for_turn_signal_deactivation_near_intersection, 0.0, 1.0); - - const double required_end_length = length_start_to_end * ratio; - double accumulated_length = 0.0; - const size_t start_idx = motion_utils::findNearestIndex(path.points, start_pose.position); - for (size_t i = start_idx; i < path.points.size() - 1; ++i) { - accumulated_length += - tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); - if (accumulated_length > required_end_length) { - return path.points.at(i).point.pose; - } + constexpr bool egos_lane_is_shifted = true; + constexpr bool is_pull_out = true; + + // In Geometric pull out, the ego stops once and then steers the wheels to the opposite direction. + // This sometimes causes the getBehaviorTurnSignalInfo method to detect the ego as stopped and + // close to complete its shift, so it wrongly turns off the blinkers, this override helps avoid + // this issue. + const bool override_ego_stopped_check = std::invoke([&]() { + if (status_.planner_type != PlannerType::GEOMETRIC) { + return false; } - // not found required end point - return end_pose; + constexpr double distance_threshold = 1.0; + const auto stop_point = status_.pull_out_path.partial_paths.front().points.back(); + const double distance_from_ego_to_stop_point = std::abs(motion_utils::calcSignedArcLength( + path.points, stop_point.point.pose.position, current_pose.position)); + return distance_from_ego_to_stop_point < distance_threshold; }); - return turn_signal; + const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( + path, shift_start_idx, shift_end_idx, current_lanes, current_shift_length, + status_.driving_forward, egos_lane_is_shifted, override_ego_stopped_check, is_pull_out); + ignore_signal_ = update_ignore_signal(closest_lanelet.id(), is_ignore); + + const auto original_signal = getPreviousModuleOutput().turn_signal_info; + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(path.points); + const auto output_turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + path, current_pose, current_seg_idx, original_signal, new_signal, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + + return output_turn_signal_info; } bool StartPlannerModule::isSafePath() const @@ -1375,7 +1389,7 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons const double drivable_area_margin = planner_data_->parameters.vehicle_width; output.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; - break; + return; } default: { const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( @@ -1389,7 +1403,7 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons ? utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info) : current_drivable_area_info; - break; + return; } } } diff --git a/planning/behavior_path_start_planner_module/src/util.cpp b/planning/behavior_path_start_planner_module/src/util.cpp index b34398083a95c..82e9d1a08e816 100644 --- a/planning/behavior_path_start_planner_module/src/util.cpp +++ b/planning/behavior_path_start_planner_module/src/util.cpp @@ -104,4 +104,40 @@ lanelet::ConstLanelets getPullOutLanes( /*forward_only_in_route*/ true); } +std::optional extractCollisionCheckSection( + const PullOutPath & path, const double collision_check_distance_from_end) +{ + PathWithLaneId full_path; + for (const auto & partial_path : path.partial_paths) { + full_path.points.insert( + full_path.points.end(), partial_path.points.begin(), partial_path.points.end()); + } + + if (full_path.points.empty()) return std::nullopt; + // Find the start index for collision check section based on the shift start pose + const auto shift_start_idx = + motion_utils::findNearestIndex(full_path.points, path.start_pose.position); + + // Find the end index for collision check section based on the end pose and collision check + // distance + const auto collision_check_end_idx = [&]() -> size_t { + const auto end_pose_offset = motion_utils::calcLongitudinalOffsetPose( + full_path.points, path.end_pose.position, collision_check_distance_from_end); + + return end_pose_offset + ? motion_utils::findNearestIndex(full_path.points, end_pose_offset->position) + : full_path.points.size() - 1; // Use the last point if offset pose is not calculable + }(); + + // Extract the collision check section from the full path + PathWithLaneId collision_check_section; + if (shift_start_idx < collision_check_end_idx) { + collision_check_section.points.assign( + full_path.points.begin() + shift_start_idx, + full_path.points.begin() + collision_check_end_idx + 1); + } + + return collision_check_section; +} + } // namespace behavior_path_planner::start_planner_utils diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index a306137795849..7a3ea3160cb24 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -68,11 +68,11 @@ BlindSpotModule::BlindSpotModule( const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id), + planner_param_{planner_param}, turn_direction_(TurnDirection::INVALID), is_over_pass_judge_line_(false) { velocity_factor_.init(PlanningBehavior::REAR_CHECK); - planner_param_ = planner_param; const auto & assigned_lanelet = planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); @@ -91,7 +91,7 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto const auto input_path = *path; - StateMachine::State current_state = state_machine_.getState(); + const auto current_state = state_machine_.getState(); RCLCPP_DEBUG( logger_, "lane_id = %ld, state = %s", lane_id_, StateMachine::toString(current_state).c_str()); @@ -110,12 +110,8 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto } const auto & interpolated_path_info = interpolated_path_info_opt.value(); - if (!first_conflicting_lanelet_) { - first_conflicting_lanelet_ = getFirstConflictingLanelet(interpolated_path_info); - } - if (!first_conflicting_lanelet_) { - RCLCPP_DEBUG(logger_, "[BlindSpotModule::run] failed to find first conflicting lanelet"); - return false; + if (!sibling_straight_lanelet_) { + sibling_straight_lanelet_ = getSiblingStraightLanelet(); } const auto stoplines_idx_opt = generateStopLine(interpolated_path_info, path); @@ -263,57 +259,24 @@ std::optional BlindSpotModule::generateInterpolatedPathInf return interpolated_path_info; } -std::optional BlindSpotModule::getFirstConflictingLanelet( - const InterpolatedPathInfo & interpolated_path_info) const +std::optional BlindSpotModule::getSiblingStraightLanelet() const { - if (!interpolated_path_info.lane_id_interval) { - return std::nullopt; - } - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); const auto assigned_lane = lanelet_map_ptr->laneletLayer.get(lane_id_); - const auto conflicting_lanes = - lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lane); - lanelet::ConstLanelets conflicting_ex_sibling_lanes{}; - lanelet::ConstLanelets sibling_lanes{}; for (const auto & prev : routing_graph_ptr->previous(assigned_lane)) { for (const auto & following : routing_graph_ptr->following(prev)) { - if (!lanelet::utils::contains(sibling_lanes, following)) { - sibling_lanes.push_back(following); - } - } - } - for (const auto & conflicting : conflicting_lanes) { - if (!lanelet::utils::contains(sibling_lanes, conflicting)) { - conflicting_ex_sibling_lanes.push_back(conflicting); - } - } - - const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); - const size_t vehicle_length_idx = static_cast( - planner_data_->vehicle_info_.max_longitudinal_offset_m / interpolated_path_info.ds); - const size_t start = - static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); - const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); - for (size_t i = start; i <= lane_end; ++i) { - const auto & pose = interpolated_path_info.path.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(pose)); - for (const auto & conflicting : conflicting_ex_sibling_lanes) { - const auto area2d = conflicting.polygon2d().basicPolygon(); - const bool is_in_polygon = bg::intersects(area2d, path_footprint); - if (is_in_polygon) { - return std::make_optional(conflicting); + if (std::string(following.attributeOr("turn_direction", "else")) == "straight") { + return following; } } } return std::nullopt; } -static std::optional getFirstPointInsidePolygonByFootprint( - const lanelet::CompoundPolygon2d & polygon, const InterpolatedPathInfo & interpolated_path_info, +static std::optional getFirstPointIntersectsLineByFootprint( + const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; @@ -321,12 +284,12 @@ static std::optional getFirstPointInsidePolygonByFootprint( const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); const size_t start = static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); - const auto area_2d = polygon.basicPolygon(); + const auto line2d = line.basicLineString(); for (auto i = start; i <= lane_end; ++i) { const auto & base_pose = path_ip.points.at(i).point.pose; const auto path_footprint = tier4_autoware_utils::transformVector( footprint, tier4_autoware_utils::pose2transform(base_pose)); - if (bg::intersects(path_footprint, area_2d)) { + if (bg::intersects(path_footprint, line2d)) { return std::make_optional(i); } } @@ -389,21 +352,47 @@ std::optional> BlindSpotModule::generateStopLine( const int margin_idx_dist = std::ceil(planner_param_.stop_line_margin / interpolated_path_info.ds); - const auto first_conflict_idx_ip_opt = getFirstPointInsidePolygonByFootprint( - first_conflicting_lanelet_.value().polygon2d(), interpolated_path_info, - planner_data_->vehicle_info_.createFootprint(0.0, 0.0), - planner_data_->vehicle_info_.max_longitudinal_offset_m); - if (!first_conflict_idx_ip_opt) { - return std::nullopt; - } - const int first_conflict_idx_ip = static_cast(first_conflict_idx_ip_opt.value()); + const auto & path_ip = interpolated_path_info.path; - const size_t stop_idx_default_ip = - static_cast(std::max(first_conflict_idx_ip - margin_idx_dist, 0)); - const size_t stop_idx_critical_ip = static_cast(first_conflict_idx_ip); + size_t stop_idx_default_ip = 0; + size_t stop_idx_critical_ip = 0; + if (sibling_straight_lanelet_) { + const auto sibling_straight_lanelet = sibling_straight_lanelet_.value(); + const auto turn_boundary_line = turn_direction_ == TurnDirection::LEFT + ? sibling_straight_lanelet.leftBound() + : sibling_straight_lanelet.rightBound(); + const auto first_conflict_idx_ip_opt = getFirstPointIntersectsLineByFootprint( + lanelet::utils::to2D(turn_boundary_line), interpolated_path_info, + planner_data_->vehicle_info_.createFootprint(0.0, 0.0), + planner_data_->vehicle_info_.max_longitudinal_offset_m); + if (!first_conflict_idx_ip_opt) { + return std::nullopt; + } + const int first_conflict_idx_ip = static_cast(first_conflict_idx_ip_opt.value()); + + stop_idx_default_ip = static_cast(std::max(first_conflict_idx_ip - margin_idx_dist, 0)); + stop_idx_critical_ip = static_cast(first_conflict_idx_ip); + } else { + // the entry point of the assigned lane + const auto & assigned_lanelet = + planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_); + const auto left_pt = assigned_lanelet.leftBound().front().basicPoint(); + const auto right_pt = assigned_lanelet.rightBound().front().basicPoint(); + const auto mid_pt = (left_pt + right_pt) / 2.0; + const geometry_msgs::msg::Point mid_point = + geometry_msgs::build().x(mid_pt.x()).y(mid_pt.y()).z(mid_pt.z()); + stop_idx_default_ip = stop_idx_critical_ip = + motion_utils::findNearestSegmentIndex(path_ip.points, mid_point); + /* + // NOTE: it is not ambiguous when the curve starts on the left/right lanelet, so this module + inserts stopline at the beginning of the lanelet for baselink + stop_idx_default_ip = stop_idx_critical_ip = static_cast(std::max(0, + static_cast(motion_utils::findNearestSegmentIndex(path_ip.points, mid_point)) - + baselink2front_dist)); + */ + } /* insert stop_point to use interpolated path*/ - const auto & path_ip = interpolated_path_info.path; const auto stopline_idx_default_opt = insertPointIndex( path_ip.points.at(stop_idx_default_ip).point.pose, path, planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index b30cdc8cc3659..fd93661b33d6b 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -106,22 +106,17 @@ class BlindSpotModule : public SceneModuleInterface private: const int64_t lane_id_; + const PlannerParam planner_param_; TurnDirection turn_direction_{TurnDirection::INVALID}; bool is_over_pass_judge_line_{false}; - std::optional first_conflicting_lanelet_{std::nullopt}; + std::optional sibling_straight_lanelet_{std::nullopt}; // Parameter - PlannerParam planner_param_; std::optional generateInterpolatedPathInfo( const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path) const; - std::optional getFirstConflictingLanelet( - const InterpolatedPathInfo & interpolated_path_info) const; - - std::optional getFirstPointConflictingLanelets( - const InterpolatedPathInfo & interpolated_path_info, - const lanelet::ConstLanelets & lanelets) const; + std::optional getSiblingStraightLanelet() const; /** * @brief Generate a stop line and insert it into the path. diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md b/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md index 8aa3415c3f329..5683295332dc2 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/README.md @@ -38,8 +38,11 @@ An object is considered by the module only if it meets all of the following cond - it is a vehicle (pedestrians are ignored); - it is moving at a velocity higher than defined by the `minimum_object_velocity` parameter; - it is not too close to the current position of the ego vehicle; +- it is not unavoidable (only if parameter `ignore_unavoidable_collisions` is set to `true`); - it is close to the ego path. +An object is considered unavoidable if it is heading towards the ego vehicle such that even if ego stops, a collision would still occur (assuming the object keeps driving in a straight line). + For the last condition, the object is considered close enough if its lateral distance from the ego path is less than the threshold parameter `minimum_object_distance_from_ego_path` plus half the width of ego and of the object (including the `extra_object_width` parameter). In addition, the value of the `hysteresis` parameter is added to the minimum distance if a stop point was inserted in the previous iteration. @@ -74,12 +77,13 @@ the stop point arc length is calculated to be the arc length of the previously f ### Module Parameters -| Parameter | Type | Description | -| --------------------------------------- | ------ | ------------------------------------------------------------------------------------------ | -| `extra_object_width` | double | [m] extra width around detected objects | -| `minimum_object_velocity` | double | [m/s] objects with a velocity bellow this value are ignored | -| `stop_distance_buffer` | double | [m] extra distance to add between the stop point and the collision point | -| `time_horizon` | double | [s] time horizon used for collision checks | -| `hysteresis` | double | [m] once a collision has been detected, this hysteresis is used on the collision detection | -| `decision_duration_buffer` | double | [s] duration between no collision being detected and the stop decision being cancelled | -| `minimum_object_distance_from_ego_path` | double | [m] minimum distance between the footprints of ego and an object to consider for collision | +| Parameter | Type | Description | +| --------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------ | +| `extra_object_width` | double | [m] extra width around detected objects | +| `minimum_object_velocity` | double | [m/s] objects with a velocity bellow this value are ignored | +| `stop_distance_buffer` | double | [m] extra distance to add between the stop point and the collision point | +| `time_horizon` | double | [s] time horizon used for collision checks | +| `hysteresis` | double | [m] once a collision has been detected, this hysteresis is used on the collision detection | +| `decision_duration_buffer` | double | [s] duration between no collision being detected and the stop decision being cancelled | +| `minimum_object_distance_from_ego_path` | double | [m] minimum distance between the footprints of ego and an object to consider for collision | +| `ignore_unavoidable_collisions` | bool | [-] if true, ignore collisions that cannot be avoided by stopping (assuming the obstacle continues going straight) | diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml b/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml index 14483093e8bb3..c18de5bd53f01 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml @@ -8,3 +8,4 @@ hysteresis: 1.0 # [m] once a collision has been detected, this hysteresis is used on the collision detection decision_duration_buffer : 1.0 # [s] duration between no collision being detected and the stop decision being cancelled minimum_object_distance_from_ego_path: 1.0 # [m] minimum distance between the footprints of ego and an object to consider for collision + ignore_unavoidable_collisions : true # if true, ignore collisions that cannot be avoided by stopping (assuming the obstacle continues going straight) diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp index 99981007b20c2..845070347d927 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp @@ -39,6 +39,8 @@ DynamicObstacleStopModuleManager::DynamicObstacleStopModuleManager(rclcpp::Node getOrDeclareParameter(node, ns + ".decision_duration_buffer"); pp.minimum_object_distance_from_ego_path = getOrDeclareParameter(node, ns + ".minimum_object_distance_from_ego_path"); + pp.ignore_unavoidable_collisions = + getOrDeclareParameter(node, ns + ".ignore_unavoidable_collisions"); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); pp.ego_lateral_offset = diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp index 0411ab2d01cfe..c95db485781f9 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp @@ -59,13 +59,37 @@ std::vector filter_predicte return obj_arc_length > ego_data.longitudinal_offset_to_first_path_idx + params.ego_longitudinal_offset + o.shape.dimensions.x / 2.0; }; - for (const auto & object : objects.objects) + const auto is_unavoidable = [&](const auto & o) { + const auto & o_pose = o.kinematics.initial_pose_with_covariance.pose; + const auto o_yaw = tf2::getYaw(o_pose.orientation); + const auto ego_yaw = tf2::getYaw(ego_data.pose.orientation); + const auto yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian(o_yaw - ego_yaw)); + const auto opposite_heading = yaw_diff > M_PI_2 + M_PI_4; + const auto collision_distance_threshold = + params.ego_lateral_offset + o.shape.dimensions.y / 2.0 + params.hysteresis; + // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_point_and_angle + const auto lat_distance = std::abs( + (o_pose.position.y - ego_data.pose.position.y) * std::cos(o_yaw) - + (o_pose.position.x - ego_data.pose.position.x) * std::sin(o_yaw)); + auto has_collision = lat_distance <= collision_distance_threshold; + if (ego_data.earliest_stop_pose) { + const auto collision_at_earliest_stop_pose = + std::abs( + (o_pose.position.y - ego_data.earliest_stop_pose->position.y) * std::cos(o_yaw) - + (o_pose.position.x - ego_data.earliest_stop_pose->position.x) * std::sin(o_yaw)) <= + collision_distance_threshold; + has_collision |= collision_at_earliest_stop_pose; + } + return opposite_heading && has_collision; + }; + for (const auto & object : objects.objects) { + const auto is_not_too_slow = object.kinematics.initial_twist_with_covariance.twist.linear.x >= + params.minimum_object_velocity; if ( - is_vehicle(object) && - object.kinematics.initial_twist_with_covariance.twist.linear.x >= - params.minimum_object_velocity && - is_in_range(object) && is_not_too_close(object)) + is_vehicle(object) && is_not_too_slow && is_in_range(object) && is_not_too_close(object) && + (!params.ignore_unavoidable_collisions || !is_unavoidable(object))) filtered_objects.push_back(object); + } return filtered_objects; } } // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp index 98b7984bab1dc..eb46a89b95ef4 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp @@ -44,7 +44,9 @@ DynamicObstacleStopModule::DynamicObstacleStopModule( const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), params_(std::move(planner_param)) { - prev_stop_decision_time_ = rclcpp::Time(int64_t{0}, clock->get_clock_type()); + prev_stop_decision_time_ = + clock->now() - + rclcpp::Duration(std::chrono::duration(params_.decision_duration_buffer)); velocity_factor_.init(PlanningBehavior::UNKNOWN); } @@ -65,10 +67,20 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position); ego_data.longitudinal_offset_to_first_path_idx = motion_utils::calcLongitudinalOffsetToSegment( ego_data.path.points, ego_data.first_path_idx, ego_data.pose.position); + const auto min_stop_distance = + motion_utils::calcDecelDistWithJerkAndAccConstraints( + planner_data_->current_velocity->twist.linear.x, 0.0, + planner_data_->current_acceleration->accel.accel.linear.x, + planner_data_->max_stop_acceleration_threshold, -planner_data_->max_stop_jerk_threshold, + planner_data_->max_stop_jerk_threshold) + .value_or(0.0); + ego_data.earliest_stop_pose = motion_utils::calcLongitudinalOffsetPose( + ego_data.path.points, ego_data.pose.position, min_stop_distance); make_ego_footprint_rtree(ego_data, params_); + const auto start_time = clock_->now(); const auto has_decided_to_stop = - (clock_->now() - prev_stop_decision_time_).seconds() < params_.decision_duration_buffer; + (start_time - prev_stop_decision_time_).seconds() < params_.decision_duration_buffer; if (!has_decided_to_stop) current_stop_pose_.reset(); double hysteresis = has_decided_to_stop ? params_.hysteresis : 0.0; const auto dynamic_obstacles = @@ -80,13 +92,6 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe const auto obstacle_forward_footprints = make_forward_footprints(dynamic_obstacles, params_, hysteresis); const auto footprints_duration_us = stopwatch.toc("footprints"); - const auto min_stop_distance = - motion_utils::calcDecelDistWithJerkAndAccConstraints( - planner_data_->current_velocity->twist.linear.x, 0.0, - planner_data_->current_acceleration->accel.accel.linear.x, - planner_data_->max_stop_acceleration_threshold, -planner_data_->max_stop_jerk_threshold, - planner_data_->max_stop_jerk_threshold) - .value_or(0.0); stopwatch.tic("collisions"); const auto collision = find_earliest_collision(ego_data, dynamic_obstacles, obstacle_forward_footprints, debug_data_); @@ -101,14 +106,13 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe ? motion_utils::calcLongitudinalOffsetPose( ego_data.path.points, *collision, -params_.stop_distance_buffer - params_.ego_longitudinal_offset) - : motion_utils::calcLongitudinalOffsetPose( - ego_data.path.points, ego_data.pose.position, min_stop_distance); + : ego_data.earliest_stop_pose; if (stop_pose) { const auto use_new_stop_pose = !has_decided_to_stop || motion_utils::calcSignedArcLength( path->points, stop_pose->position, current_stop_pose_->position) > 0.0; if (use_new_stop_pose) current_stop_pose_ = *stop_pose; - prev_stop_decision_time_ = clock_->now(); + prev_stop_decision_time_ = start_time; } } diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp index 74fd5ca818fb0..d6961bd35a22c 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp @@ -44,6 +44,7 @@ struct PlannerParam double ego_longitudinal_offset; double ego_lateral_offset; double minimum_object_distance_from_ego_path; + bool ignore_unavoidable_collisions; }; struct EgoData @@ -54,6 +55,7 @@ struct EgoData geometry_msgs::msg::Pose pose; tier4_autoware_utils::MultiPolygon2d path_footprints; Rtree rtree; + std::optional earliest_stop_pose; }; /// @brief debug data diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index b323cdf4cb5da..bd2ad1935406b 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -389,6 +389,22 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } + if (debug_data_.nearest_occlusion_triangle) { + const auto [p1, p2, p3] = debug_data_.nearest_occlusion_triangle.value(); + const auto color = debug_data_.static_occlusion ? green : red; + geometry_msgs::msg::Polygon poly; + poly.points.push_back( + geometry_msgs::build().x(p1.x).y(p1.y).z(p1.z)); + poly.points.push_back( + geometry_msgs::build().x(p2.x).y(p2.y).z(p2.z)); + poly.points.push_back( + geometry_msgs::build().x(p3.x).y(p3.y).z(p3.z)); + appendMarkerArray( + debug::createPolygonMarkerArray( + poly, "nearest_occlusion_triangle", lane_id_, now, 0.3, 0.0, 0.0, std::get<0>(color), + std::get<1>(color), std::get<2>(color)), + &debug_marker_array, now); + } if (debug_data_.traffic_light_observation) { const auto GREEN = autoware_perception_msgs::msg::TrafficSignalElement::GREEN; const auto YELLOW = autoware_perception_msgs::msg::TrafficSignalElement::AMBER; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index c2f78f28005dd..2103ef25c6cfc 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -148,17 +148,18 @@ static std::string formatOcclusionType(const IntersectionModule::OcclusionType & intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { - const auto traffic_prioritized_level = getTrafficPrioritizedLevel(); - const bool is_prioritized = - traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; - - const auto prepare_data = prepareIntersectionData(is_prioritized, path); + const auto prepare_data = prepareIntersectionData(path); if (!prepare_data) { return prepare_data.err(); } const auto [interpolated_path_info, intersection_stoplines, path_lanelets] = prepare_data.ok(); const auto & intersection_lanelets = intersection_lanelets_.value(); + // NOTE: this level is based on the updateTrafficSignalObservation() which is latest + const auto traffic_prioritized_level = getTrafficPrioritizedLevel(); + const bool is_prioritized = + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; + // ========================================================================================== // stuck detection // diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 8917b4bac579b..73736c804fb02 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -235,6 +235,10 @@ class IntersectionModule : public SceneModuleInterface std::vector occlusion_polygons; std::optional> nearest_occlusion_projection{std::nullopt}; + std::optional< + std::tuple> + nearest_occlusion_triangle{std::nullopt}; + bool static_occlusion{false}; std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; std::optional> @@ -438,6 +442,9 @@ class IntersectionModule : public SceneModuleInterface //! unavailable) std::optional> tl_id_and_point_; std::optional last_tl_valid_observation_{std::nullopt}; + + //! save previous priority level to detect change from NotPrioritized to Prioritized + TrafficPrioritizedLevel previous_prioritized_level_{TrafficPrioritizedLevel::NOT_PRIORITIZED}; /** @} */ private: @@ -544,7 +551,7 @@ class IntersectionModule : public SceneModuleInterface * To simplify modifyPathVelocityDetail(), this function is used at first */ intersection::Result prepareIntersectionData( - const bool is_prioritized, PathWithLaneId * path); + PathWithLaneId * path); /** * @brief find the associated stopline road marking of assigned lanelet diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp index 00ac333adeff1..b741d43bb025a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -63,10 +63,23 @@ IntersectionModule::getOcclusionStatus( !is_amber_or_red_or_no_tl_info_ever) ? detectOcclusion(interpolated_path_info) : NotOccluded{}; - occlusion_stop_state_machine_.setStateWithMarginTime( - std::holds_alternative(occlusion_status) ? StateMachine::State::GO - : StateMachine::STOP, - logger_.get_child("occlusion_stop"), *clock_); + + // ========================================================================================== + // if the traffic light changed from green to yellow/red, hysteresis time for occlusion is + // unnecessary + // ========================================================================================== + const auto transition_to_prioritized = + (previous_prioritized_level_ == TrafficPrioritizedLevel::NOT_PRIORITIZED && + traffic_prioritized_level != TrafficPrioritizedLevel::NOT_PRIORITIZED); + if (transition_to_prioritized) { + occlusion_stop_state_machine_.setState(StateMachine::State::GO); + } else { + occlusion_stop_state_machine_.setStateWithMarginTime( + std::holds_alternative(occlusion_status) ? StateMachine::State::GO + : StateMachine::STOP, + logger_.get_child("occlusion_stop"), *clock_); + } + const bool is_occlusion_cleared_with_margin = (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); // module's detection // distinguish if ego detected occlusion or RTC detects occlusion @@ -334,13 +347,14 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( } return nearest; }; - struct NearestOcclusionPoint + struct NearestOcclusionInterval { int64 division_index{0}; int64 point_index{0}; double dist{0.0}; geometry_msgs::msg::Point point; geometry_msgs::msg::Point projection; + geometry_msgs::msg::Point visible_end; } nearest_occlusion_point; double min_dist = std::numeric_limits::infinity(); for (unsigned division_index = 0; division_index < lane_divisions.size(); ++division_index) { @@ -370,6 +384,8 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( continue; } double acc_dist = 0.0; + bool found_min_dist_for_this_division = false; + bool is_prev_occluded = false; auto acc_dist_it = projection_it; for (auto point_it = projection_it; point_it != division.end(); point_it++) { const double dist = @@ -386,11 +402,24 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( if (acc_dist < min_dist) { min_dist = acc_dist; nearest_occlusion_point = { - division_index, std::distance(division.begin(), point_it), acc_dist, + division_index, + std::distance(division.begin(), point_it), + acc_dist, tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), - tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)}; + tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z), + tier4_autoware_utils::createPoint( + projection_it->x(), projection_it->y(), + origin.z) /* initialize with projection point at first*/}; + found_min_dist_for_this_division = true; + } else if (found_min_dist_for_this_division && is_prev_occluded) { + // although this cell is not "nearest" cell, we have found the "nearest" cell on this + // division previously in this iteration, and the iterated cells are still OCCLUDED since + // then + nearest_occlusion_point.visible_end = + tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z); } } + is_prev_occluded = (pixel == OCCLUDED); } } @@ -400,16 +429,24 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( debug_data_.nearest_occlusion_projection = std::make_pair(nearest_occlusion_point.point, nearest_occlusion_point.projection); - LineString2d ego_occlusion_line; - ego_occlusion_line.emplace_back(current_pose.position.x, current_pose.position.y); - ego_occlusion_line.emplace_back(nearest_occlusion_point.point.x, nearest_occlusion_point.point.y); + debug_data_.nearest_occlusion_triangle = std::make_tuple( + current_pose.position, nearest_occlusion_point.point, nearest_occlusion_point.visible_end); + Polygon2d ego_occlusion_triangle; + ego_occlusion_triangle.outer().emplace_back(current_pose.position.x, current_pose.position.y); + ego_occlusion_triangle.outer().emplace_back( + nearest_occlusion_point.point.x, nearest_occlusion_point.point.y); + ego_occlusion_triangle.outer().emplace_back( + nearest_occlusion_point.visible_end.x, nearest_occlusion_point.visible_end.y); + bg::correct(ego_occlusion_triangle); for (const auto & attention_object_info : object_info_manager_.allObjects()) { const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object_info->predicted_object()); - if (bg::intersects(obj_poly, ego_occlusion_line)) { + if (bg::intersects(obj_poly, ego_occlusion_triangle)) { + debug_data_.static_occlusion = false; return DynamicallyOccluded{min_dist}; } } + debug_data_.static_occlusion = true; return StaticallyOccluded{min_dist}; } } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 52c6b06541796..97d05aef26137 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -166,7 +166,7 @@ using intersection::make_ok; using intersection::Result; Result -IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithLaneId * path) +IntersectionModule::prepareIntersectionData(PathWithLaneId * path) { const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); @@ -175,6 +175,18 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); const auto & current_pose = planner_data_->current_odometry->pose; + // ========================================================================================== + // update traffic light information + // updateTrafficSignalObservation() must be called at first because other traffic signal + // fuctions use last_valid_observation_ + // ========================================================================================== + // save previous information before calling updateTrafficSignalObservation() + previous_prioritized_level_ = getTrafficPrioritizedLevel(); + updateTrafficSignalObservation(); + const auto traffic_prioritized_level = getTrafficPrioritizedLevel(); + const bool is_prioritized = + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; + // spline interpolation const auto interpolated_path_info_opt = util::generateInterpolatedPath( lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); @@ -264,13 +276,7 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL planner_data_->occupancy_grid->info.resolution); } - // ========================================================================================== - // update traffic light information - // updateTrafficSignalObservation() must be called at first to because other traffic signal - // fuctions use last_valid_observation_ - // ========================================================================================== if (has_traffic_light_) { - updateTrafficSignalObservation(); const bool is_green_solid_on = isGreenSolidOn(); if (is_green_solid_on && !initial_green_light_observed_time_) { const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); diff --git a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml index 092f1d32b27b3..1b483b95510ed 100644 --- a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml +++ b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -18,6 +18,7 @@ # if false, assume the object moves at constant velocity along *all* lanelets it currently is located in. predicted_path_min_confidence : 0.1 # when using predicted paths, ignore the ones whose confidence is lower than this value. distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane + cut_predicted_paths_beyond_red_lights: true # if true, predicted paths are cut beyond the stop line of red traffic lights overlap: minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered diff --git a/planning/behavior_velocity_out_of_lane_module/package.xml b/planning/behavior_velocity_out_of_lane_module/package.xml index 43dea2f2153ff..b61441132969c 100644 --- a/planning/behavior_velocity_out_of_lane_module/package.xml +++ b/planning/behavior_velocity_out_of_lane_module/package.xml @@ -27,6 +27,7 @@ tf2 tier4_autoware_utils tier4_planning_msgs + traffic_light_utils vehicle_info_util visualization_msgs diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index be72109db4548..e948bd74eba45 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -73,11 +73,13 @@ std::optional> object_time_to_range( if (!object_is_incoming(object_point, route_handler, range.lane)) return {}; const auto max_deviation = object.shape.dimensions.y + range.inside_distance + dist_buffer; + const auto max_lon_deviation = object.shape.dimensions.x / 2.0; auto worst_enter_time = std::optional(); auto worst_exit_time = std::optional(); for (const auto & predicted_path : object.kinematics.predicted_paths) { const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path); + if (unique_path_points.size() < 2) continue; const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds(); const auto enter_point = geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y()); @@ -121,7 +123,17 @@ std::optional> object_time_to_range( max_deviation); continue; } - // else we rely on the interpolation to estimate beyond the end of the predicted path + const auto is_last_predicted_path_point = + (exit_segment_idx + 2 == unique_path_points.size() || + enter_segment_idx + 2 == unique_path_points.size()); + const auto does_not_reach_overlap = + is_last_predicted_path_point && (std::min(exit_offset, enter_offset) > max_lon_deviation); + if (does_not_reach_overlap) { + RCLCPP_DEBUG( + logger, " * does not reach the overlap = %2.2fm | max_dev = %2.2fm\n", + std::min(exit_offset, enter_offset), max_lon_deviation); + continue; + } const auto same_driving_direction_as_ego = enter_time < exit_time; if (same_driving_direction_as_ego) { diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp new file mode 100644 index 0000000000000..4a76ef2078d63 --- /dev/null +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -0,0 +1,139 @@ +// Copyright 2023-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 "filter_predicted_objects.hpp" + +#include +#include + +#include + +#include +#include + +#include + +namespace behavior_velocity_planner::out_of_lane +{ +void cut_predicted_path_beyond_line( + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const lanelet::BasicLineString2d & stop_line, const double object_front_overhang) +{ + auto stop_line_idx = 0UL; + bool found = false; + lanelet::BasicSegment2d path_segment; + path_segment.first.x() = predicted_path.path.front().position.x; + path_segment.first.y() = predicted_path.path.front().position.y; + for (stop_line_idx = 1; stop_line_idx < predicted_path.path.size(); ++stop_line_idx) { + path_segment.second.x() = predicted_path.path[stop_line_idx].position.x; + path_segment.second.y() = predicted_path.path[stop_line_idx].position.y; + if (boost::geometry::intersects(stop_line, path_segment)) { + found = true; + break; + } + path_segment.first = path_segment.second; + } + if (found) { + auto cut_idx = stop_line_idx; + double arc_length = 0; + while (cut_idx > 0 && arc_length < object_front_overhang) { + arc_length += tier4_autoware_utils::calcDistance2d( + predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]); + --cut_idx; + } + predicted_path.path.resize(cut_idx); + } +} + +std::optional find_next_stop_line( + const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data) +{ + lanelet::ConstLanelets lanelets; + lanelet::BasicLineString2d query_line; + for (const auto & p : path.path) query_line.emplace_back(p.position.x, p.position.y); + const auto query_results = lanelet::geometry::findWithin2d( + planner_data.route_handler_->getLaneletMapPtr()->laneletLayer, query_line); + for (const auto & r : query_results) lanelets.push_back(r.second); + for (const auto & ll : lanelets) { + for (const auto & element : ll.regulatoryElementsAs()) { + const auto traffic_signal_stamped = planner_data.getTrafficSignal(element->id()); + if ( + traffic_signal_stamped.has_value() && element->stopLine().has_value() && + traffic_light_utils::isTrafficSignalStop(ll, traffic_signal_stamped.value().signal)) { + lanelet::BasicLineString2d stop_line; + for (const auto & p : *(element->stopLine())) stop_line.emplace_back(p.x(), p.y()); + return stop_line; + } + } + } + return std::nullopt; +} + +void cut_predicted_path_beyond_red_lights( + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const PlannerData & planner_data, const double object_front_overhang) +{ + const auto stop_line = find_next_stop_line(predicted_path, planner_data); + if (stop_line) cut_predicted_path_beyond_line(predicted_path, *stop_line, object_front_overhang); +} + +autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( + const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params) +{ + autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects; + filtered_objects.header = planner_data.predicted_objects->header; + for (const auto & object : planner_data.predicted_objects->objects) { + const auto is_pedestrian = + std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { + return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + }) != object.classification.end(); + if (is_pedestrian) continue; + + auto filtered_object = object; + const auto is_invalid_predicted_path = [&](const auto & predicted_path) { + const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; + const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path); + if (no_overlap_path.size() <= 1) return true; + const auto lat_offset_to_current_ego = + std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); + const auto is_crossing_ego = + lat_offset_to_current_ego <= + object.shape.dimensions.y / 2.0 + std::max( + params.left_offset + params.extra_left_offset, + params.right_offset + params.extra_right_offset); + return is_low_confidence || is_crossing_ego; + }; + if (params.objects_use_predicted_paths) { + auto & predicted_paths = filtered_object.kinematics.predicted_paths; + const auto new_end = + std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path); + predicted_paths.erase(new_end, predicted_paths.end()); + if (params.objects_cut_predicted_paths_beyond_red_lights) + for (auto & predicted_path : predicted_paths) + cut_predicted_path_beyond_red_lights( + predicted_path, planner_data, filtered_object.shape.dimensions.x); + predicted_paths.erase( + std::remove_if( + predicted_paths.begin(), predicted_paths.end(), + [](const auto & p) { return p.path.empty(); }), + predicted_paths.end()); + } + + if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty()) + filtered_objects.objects.push_back(filtered_object); + } + return filtered_objects; +} + +} // namespace behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp index 7bad4974ea9af..bb6b5f4d00005 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 TIER IV, Inc. +// Copyright 2023-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. @@ -17,56 +17,41 @@ #include "types.hpp" -#include +#include -#include +#include namespace behavior_velocity_planner::out_of_lane { +/// @brief cut a predicted path beyond the given stop line +/// @param [inout] predicted_path predicted path to cut +/// @param [in] stop_line stop line used for cutting +/// @param [in] object_front_overhang extra distance to cut ahead of the stop line +void cut_predicted_path_beyond_line( + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const lanelet::BasicLineString2d & stop_line, const double object_front_overhang); + +/// @brief find the next red light stop line along the given path +/// @param [in] path predicted path to check for a stop line +/// @param [in] planner_data planner data with stop line information +/// @return the first red light stop line found along the path (if any) +std::optional find_next_stop_line( + const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data); + +/// @brief cut predicted path beyond stop lines of red lights +/// @param [inout] predicted_path predicted path to cut +/// @param [in] planner_data planner data to get the map and traffic light information +void cut_predicted_path_beyond_red_lights( + autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const PlannerData & planner_data, const double object_front_overhang); + /// @brief filter predicted objects and their predicted paths -/// @param [in] objects predicted objects to filter +/// @param [in] planner_data planner data /// @param [in] ego_data ego data /// @param [in] params parameters /// @return filtered predicted objects autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, - const PlannerParam & params) -{ - autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects; - filtered_objects.header = objects.header; - for (const auto & object : objects.objects) { - const auto is_pedestrian = - std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { - return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; - }) != object.classification.end(); - if (is_pedestrian) continue; - - auto filtered_object = object; - const auto is_invalid_predicted_path = [&](const auto & predicted_path) { - const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; - const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path); - if (no_overlap_path.size() <= 1) return true; - const auto lat_offset_to_current_ego = - std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); - const auto is_crossing_ego = - lat_offset_to_current_ego <= - object.shape.dimensions.y / 2.0 + std::max( - params.left_offset + params.extra_left_offset, - params.right_offset + params.extra_right_offset); - return is_low_confidence || is_crossing_ego; - }; - if (params.objects_use_predicted_paths) { - auto & predicted_paths = filtered_object.kinematics.predicted_paths; - const auto new_end = - std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path); - predicted_paths.erase(new_end, predicted_paths.end()); - } - if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty()) - filtered_objects.objects.push_back(filtered_object); - } - return filtered_objects; -} - + const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params); } // namespace behavior_velocity_planner::out_of_lane #endif // FILTER_PREDICTED_OBJECTS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp index 23de809e429ec..8055f5c9106ef 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -51,6 +51,8 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) pp.objects_min_confidence = getOrDeclareParameter(node, ns + ".objects.predicted_path_min_confidence"); pp.objects_dist_buffer = getOrDeclareParameter(node, ns + ".objects.distance_buffer"); + pp.objects_cut_predicted_paths_beyond_red_lights = + getOrDeclareParameter(node, ns + ".objects.cut_predicted_paths_beyond_red_lights"); pp.overlap_min_dist = getOrDeclareParameter(node, ns + ".overlap.minimum_distance"); pp.overlap_extra_length = getOrDeclareParameter(node, ns + ".overlap.extra_length"); diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp index ef4434e235244..47020fb1cb623 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp @@ -109,13 +109,15 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto calculate_overlapping_ranges(path_footprints, path_lanelets, other_lanelets, params_); const auto calculate_overlapping_ranges_us = stopwatch.toc("calculate_overlapping_ranges"); // Calculate stop and slowdown points - stopwatch.tic("calculate_decisions"); DecisionInputs inputs; inputs.ranges = ranges; inputs.ego_data = ego_data; - inputs.objects = filter_predicted_objects(*planner_data_->predicted_objects, ego_data, params_); + stopwatch.tic("filter_predicted_objects"); + inputs.objects = filter_predicted_objects(*planner_data_, ego_data, params_); + const auto filter_predicted_objects_ms = stopwatch.toc("filter_predicted_objects"); inputs.route_handler = planner_data_->route_handler_; inputs.lanelets = other_lanelets; + stopwatch.tic("calculate_decisions"); const auto decisions = calculate_decisions(inputs, params_, logger_); const auto calculate_decisions_us = stopwatch.toc("calculate_decisions"); stopwatch.tic("calc_slowdown_points"); @@ -157,7 +159,7 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto } if (point_to_insert) { prev_inserted_point_ = point_to_insert; - RCLCPP_INFO(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id()); + RCLCPP_DEBUG(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id()); debug_data_.slowdowns = {*point_to_insert}; auto path_idx = motion_utils::findNearestSegmentIndex( path->points, point_to_insert->point.point.pose.position) + @@ -187,12 +189,13 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto "\tcalculate_lanelets = %2.0fus\n" "\tcalculate_path_footprints = %2.0fus\n" "\tcalculate_overlapping_ranges = %2.0fus\n" + "\tfilter_pred_objects = %2.0fus\n" "\tcalculate_decisions = %2.0fus\n" "\tcalc_slowdown_points = %2.0fus\n" "\tinsert_slowdown_points = %2.0fus\n", total_time_us, calculate_lanelets_us, calculate_path_footprints_us, - calculate_overlapping_ranges_us, calculate_decisions_us, calc_slowdown_points_us, - insert_slowdown_points_us); + calculate_overlapping_ranges_us, filter_predicted_objects_ms, calculate_decisions_us, + calc_slowdown_points_us, insert_slowdown_points_us); return true; } diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp index 34bd52634ce7b..d482870e5ce18 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -47,8 +47,10 @@ struct PlannerParam double ego_min_velocity; // [m/s] minimum velocity of ego used to calculate its ttc or time range bool objects_use_predicted_paths; // whether to use the objects' predicted paths - double objects_min_vel; // [m/s] objects lower than this velocity will be ignored - double objects_min_confidence; // minimum confidence to consider a predicted path + bool objects_cut_predicted_paths_beyond_red_lights; // whether to cut predicted paths beyond red + // lights' stop lines + double objects_min_vel; // [m/s] objects lower than this velocity will be ignored + double objects_min_confidence; // minimum confidence to consider a predicted path double objects_dist_buffer; // [m] distance buffer used to determine if a collision will occur in // the other lane diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 854f3c8852c29..aa9613a35dd7c 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -159,6 +159,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio } logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } void BehaviorVelocityPlannerNode::onLoadPlugin( @@ -399,6 +400,7 @@ void BehaviorVelocityPlannerNode::onTrigger( lk.unlock(); path_pub_->publish(output_path_msg); + published_time_publisher_->publish_if_subscribed(path_pub_, output_path_msg.header.stamp); stop_reason_diag_pub_->publish(planner_manager_.getStopReasonDiag()); if (debug_viz_pub_->get_subscription_count() > 0) { diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index a345f1200f721..dd6edf6ae0bc0 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -130,6 +131,8 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node const PlannerData & planner_data); std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index 21d1b5f1e3e4f..0af59417a23b2 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -26,6 +26,7 @@ #include #include +#include #include namespace behavior_velocity_planner @@ -312,6 +313,40 @@ void calculateMinAndMaxVelFromCovariance( dynamic_obstacle.max_velocity_mps = max_velocity; } +double convertDurationToDouble(const builtin_interfaces::msg::Duration & duration) +{ + return duration.sec + duration.nanosec / 1e9; +} + +// Create a path leading up to a specified prediction time +std::vector createPathToPredictionTime( + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, double prediction_time) +{ + // Calculate the number of poses to include based on the prediction time and the time step between + // poses + const double time_step_seconds = convertDurationToDouble(predicted_path.time_step); + if (time_step_seconds < std::numeric_limits::epsilon()) { + // Handle the case where time_step_seconds is zero or too close to zero + RCLCPP_WARN_STREAM( + rclcpp::get_logger("run_out: createPathToPredictionTime"), + "time_step of the path is too close to zero. Use the input path"); + const std::vector input_path( + predicted_path.path.begin(), predicted_path.path.end()); + return input_path; + } + const size_t poses_to_include = + std::min(static_cast(prediction_time / time_step_seconds), predicted_path.path.size()); + + // Construct the path to the specified prediction time + std::vector path_to_prediction_time; + path_to_prediction_time.reserve(poses_to_include); + for (size_t i = 0; i < poses_to_include; ++i) { + path_to_prediction_time.push_back(predicted_path.path[i]); + } + + return path_to_prediction_time; +} + } // namespace DynamicObstacleCreatorForObject::DynamicObstacleCreatorForObject( @@ -343,8 +378,7 @@ std::vector DynamicObstacleCreatorForObject::createDynamicObsta for (const auto & path : predicted_object.kinematics.predicted_paths) { PredictedPath predicted_path; predicted_path.confidence = path.confidence; - predicted_path.path.resize(path.path.size()); - std::copy(path.path.cbegin(), path.path.cend(), predicted_path.path.begin()); + predicted_path.path = createPathToPredictionTime(path, param_.max_prediction_time); dynamic_obstacle.predicted_paths.emplace_back(predicted_path); } diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp index fc347cb11f5cd..868c1ab12cce8 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp @@ -34,6 +34,8 @@ #include "tier4_autoware_utils/ros/self_pose_listener.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" +#include + #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" @@ -271,6 +273,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node void publishStopWatchTime(); std::unique_ptr logger_configure_; + std::unique_ptr published_time_publisher_; }; } // namespace motion_velocity_smoother diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index 5b9f84112f97a..eb8592bb99637 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -104,6 +104,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions clock_ = get_clock(); logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } void MotionVelocitySmootherNode::setupSmoother(const double wheelbase) @@ -314,6 +315,8 @@ void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & traj Trajectory publishing_trajectory = motion_utils::convertToTrajectory(trajectory); publishing_trajectory.header = base_traj_raw_ptr_->header; pub_trajectory_->publish(publishing_trajectory); + published_time_publisher_->publish_if_subscribed( + pub_trajectory_, publishing_trajectory.header.stamp); } void MotionVelocitySmootherNode::onCurrentOdometry(const Odometry::ConstSharedPtr msg) diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index 9ca2f31b6ec5a..2dbefffe67738 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -24,6 +24,8 @@ #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" +#include + #include #include #include @@ -136,6 +138,8 @@ class ObstacleAvoidancePlanner : public rclcpp::Node double vehicle_stop_margin_outside_drivable_area_; std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; }; } // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 1dcabf202970c..c1da988eb5c25 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -153,6 +153,7 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n std::bind(&ObstacleAvoidancePlanner::onParam, this, std::placeholders::_1)); logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } rcl_interfaces::msg::SetParametersResult ObstacleAvoidancePlanner::onParam( @@ -239,6 +240,7 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) const auto traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points); const auto output_traj_msg = motion_utils::convertToTrajectory(traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); + published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); return; } @@ -271,6 +273,7 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) const auto output_traj_msg = motion_utils::convertToTrajectory(full_traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); + published_time_publisher_->publish_if_subscribed(traj_pub_, output_traj_msg.header.stamp); } bool ObstacleAvoidancePlanner::isDataReady(const Path & path, rclcpp::Clock clock) const diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 22245e060b2ef..5b634823302c0 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -16,6 +16,8 @@ terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m] hold_stop_velocity_threshold: 0.01 # The maximum ego velocity to hold stopping [m/s] hold_stop_distance_threshold: 0.3 # The ego keeps stopping if the distance to stop changes within the threshold [m] + slow_down_min_acc: -1.0 # slow down min deceleration [m/ss] + slow_down_min_jerk: -1.0 # slow down min jerk [m/sss] nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index 92c3a850b3989..052e7bb721592 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -177,8 +177,8 @@ struct LongitudinalInfo limit_min_accel = node.declare_parameter("limit.min_acc"); limit_max_jerk = node.declare_parameter("limit.max_jerk"); limit_min_jerk = node.declare_parameter("limit.min_jerk"); - slow_down_min_accel = node.declare_parameter("slow_down.min_acc"); - slow_down_min_jerk = node.declare_parameter("slow_down.min_jerk"); + slow_down_min_accel = node.declare_parameter("common.slow_down_min_acc"); + slow_down_min_jerk = node.declare_parameter("common.slow_down_min_jerk"); idling_time = node.declare_parameter("common.idling_time"); min_ego_accel_for_rss = node.declare_parameter("common.min_ego_accel_for_rss"); @@ -205,8 +205,9 @@ struct LongitudinalInfo tier4_autoware_utils::updateParam(parameters, "limit.max_jerk", limit_max_jerk); tier4_autoware_utils::updateParam(parameters, "limit.min_jerk", limit_min_jerk); tier4_autoware_utils::updateParam( - parameters, "slow_down.min_accel", slow_down_min_accel); - tier4_autoware_utils::updateParam(parameters, "slow_down.min_jerk", slow_down_min_jerk); + parameters, "common.slow_down_min_accel", slow_down_min_accel); + tier4_autoware_utils::updateParam( + parameters, "common.slow_down_min_jerk", slow_down_min_jerk); tier4_autoware_utils::updateParam(parameters, "common.idling_time", idling_time); tier4_autoware_utils::updateParam( diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index a8328d536f13c..d1a9d0ff56b52 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -24,6 +24,7 @@ #include "tier4_autoware_utils/system/stop_watch.hpp" #include +#include #include #include @@ -269,6 +270,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node std::shared_ptr prev_closest_stop_obstacle_ptr_{nullptr}; std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; }; } // namespace motion_planning diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index e4e6cdf9e0873..2d1b70d1745f9 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -442,6 +442,7 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & std::bind(&ObstacleCruisePlannerNode::onParam, this, std::placeholders::_1)); logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } ObstacleCruisePlannerNode::PlanningAlgorithm ObstacleCruisePlannerNode::getPlanningAlgorithmType( @@ -538,6 +539,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms trajectory_pub_->publish(output_traj); // 8. Publish debug data + published_time_publisher_->publish_if_subscribed(trajectory_pub_, output_traj.header.stamp); publishDebugMarker(); publishDebugInfo(); diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 4e356eab6f736..d894544a67fe1 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include @@ -315,6 +316,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node } std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; }; } // namespace motion_planning diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 1b34e85b87a50..8f57b3e0f051a 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -246,6 +246,7 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod createSubscriptionOptions(this)); logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } void ObstacleStopPlannerNode::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) @@ -330,6 +331,7 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), 3000, "Backward path is NOT supported. publish input as it is."); pub_trajectory_->publish(*input_msg); + published_time_publisher_->publish_if_subscribed(pub_trajectory_, input_msg->header.stamp); return; } @@ -380,6 +382,7 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m trajectory.header = input_msg->header; pub_trajectory_->publish(trajectory); + published_time_publisher_->publish_if_subscribed(pub_trajectory_, trajectory.header.stamp); Float64Stamped processing_time_ms; processing_time_ms.stamp = now(); diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp index cbdd390183d61..63eec0f2cf378 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp @@ -21,6 +21,7 @@ #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include +#include #include #include @@ -104,6 +105,8 @@ class ObstacleVelocityLimiterNode : public rclcpp::Node bool validInputs(); std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; }; } // namespace obstacle_velocity_limiter diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp index 086d76e9ef0fe..ea08c2f7cf552 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -82,6 +82,7 @@ ObstacleVelocityLimiterNode::ObstacleVelocityLimiterNode(const rclcpp::NodeOptio add_on_set_parameters_callback([this](const auto & params) { return onParameter(params); }); logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } rcl_interfaces::msg::SetParametersResult ObstacleVelocityLimiterNode::onParameter( @@ -221,6 +222,7 @@ void ObstacleVelocityLimiterNode::onTrajectory(const Trajectory::ConstSharedPtr safe_trajectory.header.stamp = now(); pub_trajectory_->publish(safe_trajectory); + published_time_publisher_->publish_if_subscribed(pub_trajectory_, safe_trajectory.header.stamp); const auto t_end = std::chrono::system_clock::now(); const auto runtime = std::chrono::duration_cast(t_end - t_start); diff --git a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp index 364e1e3cc43d8..eb2d651cc967d 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp @@ -23,6 +23,8 @@ #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include + #include #include #include @@ -112,6 +114,8 @@ class ElasticBandSmoother : public rclcpp::Node const std::vector & optimized_points) const; std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; }; } // namespace path_smoother diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/path_smoother/src/elastic_band_smoother.cpp index a81a92f3fcc96..6fb732309da4b 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -108,6 +108,7 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option std::bind(&ElasticBandSmoother::onParam, this, std::placeholders::_1)); logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } rcl_interfaces::msg::SetParametersResult ElasticBandSmoother::onParam( @@ -171,6 +172,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) const auto output_traj_msg = motion_utils::convertToTrajectory(traj_points, path_ptr->header); traj_pub_->publish(output_traj_msg); path_pub_->publish(*path_ptr); + published_time_publisher_->publish_if_subscribed(path_pub_, path_ptr->header.stamp); return; } @@ -225,6 +227,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) traj_pub_->publish(output_traj_msg); const auto output_path_msg = trajectory_utils::create_path(*path_ptr, full_traj_points); path_pub_->publish(output_path_msg); + published_time_publisher_->publish_if_subscribed(path_pub_, path_ptr->header.stamp); } bool ElasticBandSmoother::isDataReady(const Path & path, rclcpp::Clock clock) const diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/planning_validator/planning_validator.hpp index 4e27def784b12..36954c267b2a9 100644 --- a/planning/planning_validator/include/planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/planning_validator/planning_validator.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -136,6 +137,8 @@ class PlanningValidator : public rclcpp::Node std::unique_ptr logger_configure_; + std::unique_ptr published_time_publisher_; + StopWatch stop_watch_; }; } // namespace planning_validator diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index 8b8b7d4566f08..1e5f6fc53acf0 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -48,6 +48,7 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options) setupParameters(); logger_configure_ = std::make_unique(this); + published_time_publisher_ = std::make_unique(this); } void PlanningValidator::setupParameters() @@ -219,6 +220,7 @@ void PlanningValidator::publishTrajectory() // Validation check is all green. Publish the trajectory. if (isAllValid(validation_status_)) { pub_traj_->publish(*current_trajectory_); + published_time_publisher_->publish_if_subscribed(pub_traj_, current_trajectory_->header.stamp); previous_published_trajectory_ = current_trajectory_; return; } @@ -227,6 +229,7 @@ void PlanningValidator::publishTrajectory() if (invalid_trajectory_handling_type_ == InvalidTrajectoryHandlingType::PUBLISH_AS_IT_IS) { pub_traj_->publish(*current_trajectory_); + published_time_publisher_->publish_if_subscribed(pub_traj_, current_trajectory_->header.stamp); RCLCPP_ERROR(get_logger(), "Caution! Invalid Trajectory published."); return; } @@ -239,6 +242,8 @@ void PlanningValidator::publishTrajectory() if (invalid_trajectory_handling_type_ == InvalidTrajectoryHandlingType::USE_PREVIOUS_RESULT) { if (previous_published_trajectory_) { pub_traj_->publish(*previous_published_trajectory_); + published_time_publisher_->publish_if_subscribed( + pub_traj_, previous_published_trajectory_->header.stamp); RCLCPP_ERROR(get_logger(), "Invalid Trajectory detected. Use previous trajectory."); return; } diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 68be0ac33e934..8cec5073efb6a 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -643,12 +643,22 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence( return lanelet_sequence; } - lanelet::ConstLanelets lanelet_sequence_forward = - getLaneletSequenceAfter(lanelet, forward_distance, only_route_lanes); + const lanelet::ConstLanelets lanelet_sequence_forward = std::invoke([&]() { + if (only_route_lanes) { + return getLaneletSequenceAfter(lanelet, forward_distance, only_route_lanes); + } else if (lanelet::utils::contains(shoulder_lanelets_, lanelet)) { + return getShoulderLaneletSequenceAfter(lanelet, forward_distance); + } + return lanelet::ConstLanelets{}; + }); const lanelet::ConstLanelets lanelet_sequence_backward = std::invoke([&]() { const auto arc_coordinate = lanelet::utils::getArcCoordinates({lanelet}, current_pose); if (arc_coordinate.length < backward_distance) { - return getLaneletSequenceUpTo(lanelet, backward_distance, only_route_lanes); + if (only_route_lanes) { + return getLaneletSequenceUpTo(lanelet, backward_distance, only_route_lanes); + } else if (lanelet::utils::contains(shoulder_lanelets_, lanelet)) { + return getShoulderLaneletSequenceUpTo(lanelet, backward_distance); + } } return lanelet::ConstLanelets{}; }); @@ -748,12 +758,18 @@ lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequenceAfter( double length = 0; lanelet::ConstLanelet current_lanelet = lanelet; + std::set searched_ids{}; while (rclcpp::ok() && length < min_length) { lanelet::ConstLanelet next_lanelet; if (!getFollowingShoulderLanelet(current_lanelet, &next_lanelet)) { break; } lanelet_sequence_forward.push_back(next_lanelet); + if (searched_ids.find(next_lanelet.id()) != searched_ids.end()) { + // loop shoulder detected + break; + } + searched_ids.insert(next_lanelet.id()); current_lanelet = next_lanelet; length += static_cast(boost::geometry::length(next_lanelet.centerline().basicLineString())); @@ -784,6 +800,7 @@ lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequenceUpTo( double length = 0; lanelet::ConstLanelet current_lanelet = lanelet; + std::set searched_ids{}; while (rclcpp::ok() && length < min_length) { lanelet::ConstLanelet prev_lanelet; if (!getPreviousShoulderLanelet(current_lanelet, &prev_lanelet)) { @@ -791,6 +808,11 @@ lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequenceUpTo( } lanelet_sequence_backward.insert(lanelet_sequence_backward.begin(), prev_lanelet); + if (searched_ids.find(prev_lanelet.id()) != searched_ids.end()) { + // loop shoulder detected + break; + } + searched_ids.insert(prev_lanelet.id()); current_lanelet = prev_lanelet; length += static_cast(boost::geometry::length(prev_lanelet.centerline().basicLineString())); @@ -2133,24 +2155,17 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints( lanelet::routing::LaneletPath shortest_path; bool is_route_found = false; - lanelet::routing::LaneletPath drivable_lane_path; - bool drivable_lane_path_found = false; - double shortest_path_length2d = std::numeric_limits::max(); + double smallest_angle_diff = std::numeric_limits::max(); + constexpr double yaw_threshold = M_PI / 2.0; for (const auto & st_llt : start_lanelets) { // check if the angle difference between start_checkpoint and start lanelet center line // orientation is in yaw_threshold range - double yaw_threshold = M_PI / 2.0; - bool is_proper_angle = false; - { - double lanelet_angle = lanelet::utils::getLaneletAngle(st_llt, start_checkpoint.position); - double pose_yaw = tf2::getYaw(start_checkpoint.orientation); - double angle_diff = std::abs(autoware_utils::normalize_radian(lanelet_angle - pose_yaw)); - - if (angle_diff <= std::abs(yaw_threshold)) { - is_proper_angle = true; - } - } + double lanelet_angle = lanelet::utils::getLaneletAngle(st_llt, start_checkpoint.position); + double pose_yaw = tf2::getYaw(start_checkpoint.orientation); + double angle_diff = std::abs(autoware_utils::normalize_radian(lanelet_angle - pose_yaw)); + + bool is_proper_angle = angle_diff <= std::abs(yaw_threshold); optional_route = routing_graph_ptr_->getRoute(st_llt, goal_lanelet, 0); if (!optional_route || !is_proper_angle) { @@ -2161,34 +2176,28 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints( << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl << " - start lane id: " << st_llt.id() << std::endl << " - goal lane id: " << goal_lanelet.id() << std::endl); - } else { - is_route_found = true; - - if (optional_route->length2d() < shortest_path_length2d) { - shortest_path_length2d = optional_route->length2d(); - shortest_path = optional_route->shortestPath(); - start_lanelet = st_llt; - } + continue; + } + is_route_found = true; + if (angle_diff < smallest_angle_diff) { + smallest_angle_diff = angle_diff; + shortest_path = optional_route->shortestPath(); + start_lanelet = st_llt; } } if (is_route_found) { lanelet::routing::LaneletPath path; - if (consider_no_drivable_lanes) { - bool shortest_path_has_no_drivable_lane = hasNoDrivableLaneInPath(shortest_path); - if (shortest_path_has_no_drivable_lane) { + path = [&]() -> lanelet::routing::LaneletPath { + if (!consider_no_drivable_lanes) return shortest_path; + lanelet::routing::LaneletPath drivable_lane_path; + bool drivable_lane_path_found = false; + if (hasNoDrivableLaneInPath(shortest_path)) { drivable_lane_path_found = findDrivableLanePath(start_lanelet, goal_lanelet, drivable_lane_path); } - - if (drivable_lane_path_found) { - path = drivable_lane_path; - } else { - path = shortest_path; - } - } else { - path = shortest_path; - } + return (drivable_lane_path_found) ? drivable_lane_path : shortest_path; + }(); path_lanelets->reserve(path.size()); for (const auto & llt : path) { diff --git a/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp b/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp index 94969570d6b53..3651f6c76b8bc 100644 --- a/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp +++ b/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp @@ -16,6 +16,7 @@ #define SCENARIO_SELECTOR__SCENARIO_SELECTOR_NODE_HPP_ #include +#include #include #include @@ -91,6 +92,7 @@ class ScenarioSelectorNode : public rclcpp::Node std::shared_ptr routing_graph_ptr_; std::shared_ptr traffic_rules_ptr_; std::shared_ptr route_handler_; + std::unique_ptr published_time_publisher_; // Parameters double update_rate_; diff --git a/planning/scenario_selector/package.xml b/planning/scenario_selector/package.xml index 53c29613a1de8..b985cac0b6ae7 100644 --- a/planning/scenario_selector/package.xml +++ b/planning/scenario_selector/package.xml @@ -27,6 +27,7 @@ tf2 tf2_geometry_msgs tf2_ros + tier4_autoware_utils tier4_planning_msgs ros2cli diff --git a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp index 734ff11aaf4a1..7cc251a03296c 100644 --- a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp +++ b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp @@ -320,6 +320,7 @@ void ScenarioSelectorNode::publishTrajectory( const auto delay_sec = (now - msg->header.stamp).seconds(); if (delay_sec <= th_max_message_delay_sec_) { pub_trajectory_->publish(*msg); + published_time_publisher_->publish_if_subscribed(pub_trajectory_, msg->header.stamp); } else { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), @@ -372,6 +373,7 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&ScenarioSelectorNode::onTimer, this)); + published_time_publisher_ = std::make_unique(this); } #include diff --git a/sensing/image_diagnostics/package.xml b/sensing/image_diagnostics/package.xml index c613ea5e18902..ea57285e81aba 100644 --- a/sensing/image_diagnostics/package.xml +++ b/sensing/image_diagnostics/package.xml @@ -4,9 +4,11 @@ image_diagnostics 0.1.0 The image_diagnostics_package - dainguyen + Dai Nguyen Apache License 2.0 + Dai Nguyen + ament_cmake_auto cv_bridge diff --git a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml index eac423f94fa78..10f192696aabb 100644 --- a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml +++ b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - gyro_bias_threshold: 0.0015 # [rad/s] + gyro_bias_threshold: 0.003 # [rad/s] timer_callback_interval_sec: 0.5 # [sec] diagnostics_updater_interval_sec: 0.5 # [sec] straight_motion_ang_vel_upper_limit: 0.015 # [rad/s] diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md index 9dcc8b1a03715..61f65bc40f32d 100644 --- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -22,14 +22,14 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ### Core Parameters -| Name | Type | Default Value | Description | -| ------------------------- | ------- | ------------- | ------------------------------------------------------------------------------------------------------------------------------- | -| `distance_ratio` | double | 1.03 | | -| `object_length_threshold` | double | 0.1 | | -| `num_points_threshold` | int | 4 | | -| `max_rings_num` | uint_16 | 128 | | -| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` | -| `publish_excluded_points` | bool | false | Flag to publish excluded pointcloud for debugging purpose. Due to performance concerns, please set to false during experiments. | +| Name | Type | Default Value | Description | +| ---------------------------- | ------- | ------------- | -------------------------------------------------------------------------------------------------------- | +| `distance_ratio` | double | 1.03 | | +| `object_length_threshold` | double | 0.1 | | +| `num_points_threshold` | int | 4 | | +| `max_rings_num` | uint_16 | 128 | | +| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` | +| `publish_outlier_pointcloud` | bool | false | Flag to publish outlier pointcloud. Due to performance concerns, please set to false during experiments. | ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp index d1560cccd0b15..84b056bec37b7 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp @@ -126,6 +126,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node double timeout_sec_ = 0.1; bool publish_synchronized_pointcloud_; + bool keep_input_frame_in_synchronized_pointcloud_; std::string synchronized_pointcloud_postfix_; std::set not_subscribed_topic_names_; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp index 47c0f2b403faa..1f23c975a88af 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp @@ -81,6 +81,7 @@ // Include tier4 autoware utils #include +#include #include namespace pointcloud_preprocessor @@ -178,6 +179,7 @@ class Filter : public rclcpp::Node /** \brief processing time publisher. **/ std::unique_ptr> stop_watch_ptr_; std::unique_ptr debug_publisher_; + std::unique_ptr published_time_publisher_; /** \brief Virtual abstract filter method. To be implemented by every child. * \param input the input point cloud dataset. diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index 57ed7a508ca54..c871f1acbe5b9 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -44,14 +44,14 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter private: /** \brief publisher of excluded pointcloud for debug reason. **/ - rclcpp::Publisher::SharedPtr excluded_points_publisher_; + rclcpp::Publisher::SharedPtr outlier_pointcloud_publisher_; double distance_ratio_; double object_length_threshold_; int num_points_threshold_; uint16_t max_rings_num_; size_t max_points_num_per_ring_; - bool publish_excluded_points_; + bool publish_outlier_pointcloud_; /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -73,8 +73,10 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter return x * x + y * y + z * z >= object_length_threshold_ * object_length_threshold_; } - PointCloud2 extractExcludedPoints( - const PointCloud2 & input, const PointCloud2 & output, float epsilon); + + void setUpPointCloudFormat( + const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size, + size_t num_fields); public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp index 4a36c32980b0e..9330b1e998b5b 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp @@ -138,6 +138,7 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node /** \brief Output TF frame the concatenated points should be transformed to. */ std::string output_frame_; + bool keep_input_frame_in_synchronized_pointcloud_; /** \brief Input point cloud topics. */ // XmlRpc::XmlRpcValue input_topics_; diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index 936c52ef81b37..73f40cc4b44b0 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -8,6 +8,7 @@ Yukihiro Saito Shunsuke Miura Kyoichi Sugahara + Dai Nguyen Apache License 2.0 diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index f6407c532b5f8..402ceb65f6594 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -112,7 +112,9 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro } // Check if publish synchronized pointcloud - publish_synchronized_pointcloud_ = declare_parameter("publish_synchronized_pointcloud", false); + publish_synchronized_pointcloud_ = declare_parameter("publish_synchronized_pointcloud", true); + keep_input_frame_in_synchronized_pointcloud_ = + declare_parameter("keep_input_frame_in_synchronized_pointcloud", true); synchronized_pointcloud_postfix_ = declare_parameter("synchronized_pointcloud_postfix", "pointcloud"); } @@ -145,10 +147,10 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro // Subscribers { - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( get_logger(), "Subscribing to " << input_topics_.size() << " user given topics as inputs:"); for (auto & input_topic : input_topics_) { - RCLCPP_INFO_STREAM(get_logger(), " - " << input_topic); + RCLCPP_DEBUG_STREAM(get_logger(), " - " << input_topic); } // Subscribe to the filters @@ -397,10 +399,23 @@ PointCloudConcatenateDataSynchronizerComponent::combineClouds( pcl::concatenatePointCloud( *concat_cloud_ptr, *transformed_delay_compensated_cloud_ptr, *concat_cloud_ptr); } - // gather transformed clouds - transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp; - transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_; - transformed_clouds[e.first] = transformed_delay_compensated_cloud_ptr; + // convert to original sensor frame if necessary + bool need_transform_to_sensor_frame = (e.second->header.frame_id != output_frame_); + if (keep_input_frame_in_synchronized_pointcloud_ && need_transform_to_sensor_frame) { + sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr_in_sensor_frame( + new sensor_msgs::msg::PointCloud2()); + pcl_ros::transformPointCloud( + (std::string)e.second->header.frame_id, *transformed_delay_compensated_cloud_ptr, + *transformed_cloud_ptr_in_sensor_frame, *tf2_buffer_); + transformed_cloud_ptr_in_sensor_frame->header.stamp = oldest_stamp; + transformed_cloud_ptr_in_sensor_frame->header.frame_id = e.second->header.frame_id; + transformed_clouds[e.first] = transformed_cloud_ptr_in_sensor_frame; + } else { + transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp; + transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_; + transformed_clouds[e.first] = transformed_delay_compensated_cloud_ptr; + } + } else { not_subscribed_topic_names_.insert(e.first); } @@ -418,20 +433,6 @@ void PointCloudConcatenateDataSynchronizerComponent::publish() const auto & transformed_raw_points = PointCloudConcatenateDataSynchronizerComponent::combineClouds(concat_cloud_ptr); - for (const auto & e : cloud_stdmap_) { - if (e.second != nullptr) { - if (debug_publisher_) { - const auto pipeline_latency_ms = - std::chrono::duration( - std::chrono::nanoseconds( - (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) - .count(); - debug_publisher_->publish( - "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms); - } - } - } - // publish concatenated pointcloud if (concat_cloud_ptr) { auto output = std::make_unique(*concat_cloud_ptr); @@ -469,6 +470,19 @@ void PointCloudConcatenateDataSynchronizerComponent::publish() debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } + for (const auto & e : cloud_stdmap_) { + if (e.second != nullptr) { + if (debug_publisher_) { + const auto pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms); + } + } + } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp index d0a4fc892e940..46c9e681c3de2 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp @@ -281,6 +281,20 @@ void PointCloudConcatenationComponent::publish() checkSyncStatus(); combineClouds(concat_cloud_ptr); + for (const auto & e : cloud_stdmap_) { + if (e.second != nullptr) { + if (debug_publisher_) { + const auto pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms); + } + } + } + // publish concatenated pointcloud if (concat_cloud_ptr) { auto output = std::make_unique(*concat_cloud_ptr); diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp index d5843be395adf..5584fd65c9acb 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -77,7 +77,7 @@ pointcloud_preprocessor::Filter::Filter( latched_indices_ = static_cast(declare_parameter("latched_indices", false)); approximate_sync_ = static_cast(declare_parameter("approximate_sync", false)); - RCLCPP_INFO_STREAM( + RCLCPP_DEBUG_STREAM( this->get_logger(), "Filter (as Component) successfully created with the following parameters:" << std::endl @@ -102,6 +102,7 @@ pointcloud_preprocessor::Filter::Filter( set_param_res_filter_ = this->add_on_set_parameters_callback( std::bind(&Filter::filterParamCallback, this, std::placeholders::_1)); + published_time_publisher_ = std::make_unique(this); RCLCPP_DEBUG(this->get_logger(), "[Filter Constructor] successfully created."); } @@ -125,7 +126,7 @@ void pointcloud_preprocessor::Filter::subscribe(const std::string & filter_name) // each time a child class supports the faster version. // When all the child classes support the faster version, this workaround is deleted. std::set supported_nodes = { - "CropBoxFilter", "RingOutlierFilter", "VoxelGridDownsampleFilter"}; + "CropBoxFilter", "RingOutlierFilter", "VoxelGridDownsampleFilter", "ScanGroundFilter"}; auto callback = supported_nodes.find(filter_name) != supported_nodes.end() ? &Filter::faster_input_indices_callback : &Filter::input_indices_callback; @@ -192,6 +193,7 @@ void pointcloud_preprocessor::Filter::computePublish( // Publish a boost shared ptr pub_output_->publish(std::move(output)); + published_time_publisher_->publish_if_subscribed(pub_output_, input->header.stamp); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -456,6 +458,7 @@ void pointcloud_preprocessor::Filter::faster_input_indices_callback( output->header.stamp = cloud->header.stamp; pub_output_->publish(std::move(output)); + published_time_publisher_->publish_if_subscribed(pub_output_, cloud->header.stamp); } // TODO(sykwer): Temporary Implementation: Remove this interface when all the filter nodes conform diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp index e0cc9a5964a09..ccceae9ab5186 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp @@ -139,7 +139,8 @@ void DualReturnOutlierFilterComponent::filter( } } - uint32_t horizontal_res = static_cast((max_azimuth - min_azimuth) / horizontal_bins); + uint32_t horizontal_resolution = + static_cast((max_azimuth - min_azimuth) / horizontal_bins); pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); pcl_output->points.reserve(pcl_input->points.size()); @@ -231,7 +232,8 @@ void DualReturnOutlierFilterComponent::filter( continue; } while ((uint)deleted_azimuths[current_deleted_index] < - ((i + static_cast(min_azimuth / horizontal_res) + 1) * horizontal_res) && + ((i + static_cast(min_azimuth / horizontal_resolution) + 1) * + horizontal_resolution) && current_deleted_index < (deleted_azimuths.size() - 1)) { noise_frequency[i] = noise_frequency[i] + 1; current_deleted_index++; @@ -240,7 +242,8 @@ void DualReturnOutlierFilterComponent::filter( while ((temp_segment.points[current_temp_segment_index].azimuth < 0.f ? 0.f : temp_segment.points[current_temp_segment_index].azimuth) < - ((i + 1 + static_cast(min_azimuth / horizontal_res)) * horizontal_res) && + ((i + 1 + static_cast(min_azimuth / horizontal_resolution)) * + horizontal_resolution) && current_temp_segment_index < (temp_segment.points.size() - 1)) { if (noise_frequency[i] < weak_first_local_noise_threshold_) { pcl_output->points.push_back(temp_segment.points[current_temp_segment_index]); diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index e0c8443cfe57b..762c62dc03b39 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -33,8 +33,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions using tier4_autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "ring_outlier_filter"); - excluded_points_publisher_ = - this->create_publisher("debug/ring_outlier_filter", 1); + outlier_pointcloud_publisher_ = + this->create_publisher("debug/ring_outlier_filter", 1); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } @@ -48,8 +48,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions max_rings_num_ = static_cast(declare_parameter("max_rings_num", 128)); max_points_num_per_ring_ = static_cast(declare_parameter("max_points_num_per_ring", 4000)); - publish_excluded_points_ = - static_cast(declare_parameter("publish_excluded_points", false)); + publish_outlier_pointcloud_ = + static_cast(declare_parameter("publish_outlier_pointcloud", false)); } using std::placeholders::_1; @@ -73,6 +73,14 @@ void RingOutlierFilterComponent::faster_filter( output.data.resize(output.point_step * input->width); size_t output_size = 0; + // Set up the noise points cloud, if noise points are to be published. + PointCloud2 outlier_points; + size_t outlier_points_size = 0; + if (publish_outlier_pointcloud_) { + outlier_points.point_step = sizeof(PointXYZI); + outlier_points.data.resize(outlier_points.point_step * input->width); + } + const auto ring_offset = input->fields.at(static_cast(autoware_point_types::PointIndex::Ring)).offset; const auto azimuth_offset = @@ -153,6 +161,27 @@ void RingOutlierFilterComponent::faster_filter( output_size += output.point_step; } + } else if (publish_outlier_pointcloud_) { + for (int i = walk_first_idx; i <= walk_last_idx; i++) { + auto outlier_ptr = + reinterpret_cast(&outlier_points.data[outlier_points_size]); + auto input_ptr = + reinterpret_cast(&input->data[indices[walk_first_idx]]); + if (transform_info.need_transform) { + Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); + p = transform_info.eigen_transform * p; + outlier_ptr->x = p[0]; + outlier_ptr->y = p[1]; + outlier_ptr->z = p[2]; + } else { + *outlier_ptr = *input_ptr; + } + const float & intensity = *reinterpret_cast( + &input->data[indices[walk_first_idx] + intensity_offset]); + outlier_ptr->intensity = intensity; + + outlier_points_size += outlier_points.point_step; + } } walk_first_idx = idx + 1; @@ -182,37 +211,32 @@ void RingOutlierFilterComponent::faster_filter( output_size += output.point_step; } + } else if (publish_outlier_pointcloud_) { + for (int i = walk_first_idx; i < walk_last_idx; i++) { + auto outlier_ptr = reinterpret_cast(&outlier_points.data[outlier_points_size]); + auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + if (transform_info.need_transform) { + Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); + p = transform_info.eigen_transform * p; + outlier_ptr->x = p[0]; + outlier_ptr->y = p[1]; + outlier_ptr->z = p[2]; + } else { + *outlier_ptr = *input_ptr; + } + const float & intensity = + *reinterpret_cast(&input->data[indices[i] + intensity_offset]); + outlier_ptr->intensity = intensity; + outlier_points_size += outlier_points.point_step; + } } } - output.data.resize(output_size); - - // Note that `input->header.frame_id` is data before converted when `transform_info.need_transform - // == true` - output.header.frame_id = !tf_input_frame_.empty() ? tf_input_frame_ : tf_input_orig_frame_; - - output.height = 1; - output.width = static_cast(output.data.size() / output.height / output.point_step); - output.is_bigendian = input->is_bigendian; - output.is_dense = input->is_dense; - - // set fields - sensor_msgs::PointCloud2Modifier pcd_modifier(output); - constexpr int num_fields = 4; - pcd_modifier.setPointCloud2Fields( - num_fields, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, - sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, - "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); + setUpPointCloudFormat(input, output, output_size, /*num_fields=*/4); - if (publish_excluded_points_) { - auto excluded_points = extractExcludedPoints(*input, output, 0.01); - // set fields - sensor_msgs::PointCloud2Modifier excluded_pcd_modifier(excluded_points); - excluded_pcd_modifier.setPointCloud2Fields( - num_fields, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, - sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, - "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); - excluded_points_publisher_->publish(excluded_points); + if (publish_outlier_pointcloud_) { + setUpPointCloudFormat(input, outlier_points, outlier_points_size, /*num_fields=*/4); + outlier_pointcloud_publisher_->publish(outlier_points); } // add processing time for debug @@ -260,9 +284,9 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba if (get_param(p, "num_points_threshold", num_points_threshold_)) { RCLCPP_DEBUG(get_logger(), "Setting new num_points_threshold to: %d.", num_points_threshold_); } - if (get_param(p, "publish_excluded_points", publish_excluded_points_)) { + if (get_param(p, "publish_outlier_pointcloud", publish_outlier_pointcloud_)) { RCLCPP_DEBUG( - get_logger(), "Setting new publish_excluded_points to: %d.", publish_excluded_points_); + get_logger(), "Setting new publish_outlier_pointcloud to: %d.", publish_outlier_pointcloud_); } rcl_interfaces::msg::SetParametersResult result; @@ -272,51 +296,27 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba return result; } -sensor_msgs::msg::PointCloud2 RingOutlierFilterComponent::extractExcludedPoints( - const sensor_msgs::msg::PointCloud2 & input, const sensor_msgs::msg::PointCloud2 & output, - float epsilon) +void RingOutlierFilterComponent::setUpPointCloudFormat( + const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size, + size_t num_fields) { - // Convert ROS PointCloud2 message to PCL point cloud for easier manipulation - pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud); - pcl::PointCloud::Ptr output_cloud(new pcl::PointCloud); - pcl::fromROSMsg(input, *input_cloud); - pcl::fromROSMsg(output, *output_cloud); - - pcl::PointCloud::Ptr excluded_points(new pcl::PointCloud); - - pcl::search::Search::Ptr tree; - if (output_cloud->isOrganized()) { - tree.reset(new pcl::search::OrganizedNeighbor()); - } else { - tree.reset(new pcl::search::KdTree(false)); - } - tree->setInputCloud(output_cloud); - std::vector nn_indices(1); - std::vector nn_distances(1); - for (const auto & point : input_cloud->points) { - if (!tree->nearestKSearch(point, 1, nn_indices, nn_distances)) { - continue; - } - if (nn_distances[0] > epsilon) { - excluded_points->points.push_back(point); - } - } - - sensor_msgs::msg::PointCloud2 excluded_points_msg; - pcl::toROSMsg(*excluded_points, excluded_points_msg); - - // Set the metadata for the excluded points message based on the input cloud - excluded_points_msg.height = 1; - excluded_points_msg.width = - static_cast(output.data.size() / output.height / output.point_step); - excluded_points_msg.row_step = static_cast(output.data.size() / output.height); - excluded_points_msg.is_bigendian = input.is_bigendian; - excluded_points_msg.is_dense = input.is_dense; - excluded_points_msg.header = input.header; - excluded_points_msg.header.frame_id = + formatted_points.data.resize(points_size); + // Note that `input->header.frame_id` is data before converted when `transform_info.need_transform + // == true` + formatted_points.header.frame_id = !tf_input_frame_.empty() ? tf_input_frame_ : tf_input_orig_frame_; - - return excluded_points_msg; + formatted_points.data.resize(formatted_points.point_step * input->width); + formatted_points.height = 1; + formatted_points.width = + static_cast(formatted_points.data.size() / formatted_points.point_step); + formatted_points.is_bigendian = input->is_bigendian; + formatted_points.is_dense = input->is_dense; + + sensor_msgs::PointCloud2Modifier pcd_modifier(formatted_points); + pcd_modifier.setPointCloud2Fields( + num_fields, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, + sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, + "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); } } // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp index 8f02721a67cfb..7aae6c937f4a9 100644 --- a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -58,7 +58,9 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( std::string synchronized_pointcloud_postfix; { output_frame_ = static_cast(declare_parameter("output_frame", "")); - if (output_frame_.empty()) { + keep_input_frame_in_synchronized_pointcloud_ = + static_cast(declare_parameter("keep_input_frame_in_synchronized_pointcloud", false)); + if (output_frame_.empty() && !keep_input_frame_in_synchronized_pointcloud_) { RCLCPP_ERROR(get_logger(), "Need an 'output_frame' parameter to be set before continuing!"); return; } @@ -351,7 +353,18 @@ PointCloudDataSynchronizerComponent::synchronizeClouds() pcl_ros::transformPointCloud( adjust_to_old_data_transform, *transformed_cloud_ptr, *transformed_delay_compensated_cloud_ptr); - + // transform to sensor frame if needed + bool need_transform_to_sensor_frame = (e.second->header.frame_id != output_frame_); + if (keep_input_frame_in_synchronized_pointcloud_ && need_transform_to_sensor_frame) { + sensor_msgs::msg::PointCloud2::SharedPtr + transformed_delay_compensated_cloud_ptr_in_input_frame( + new sensor_msgs::msg::PointCloud2()); + transformPointCloud( + transformed_delay_compensated_cloud_ptr, + transformed_delay_compensated_cloud_ptr_in_input_frame, e.second->header.frame_id); + transformed_delay_compensated_cloud_ptr = + transformed_delay_compensated_cloud_ptr_in_input_frame; + } // gather transformed clouds transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp; transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_; @@ -374,6 +387,15 @@ void PointCloudDataSynchronizerComponent::publish() // publish transformed raw pointclouds for (const auto & e : transformed_raw_points) { if (e.second) { + if (debug_publisher_) { + const auto pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms); + } auto output = std::make_unique(*e.second); transformed_raw_pc_publisher_map_[e.first]->publish(std::move(output)); } else { diff --git a/system/component_state_monitor/config/topics.yaml b/system/component_state_monitor/config/topics.yaml index f8c6f9685f19a..f056892849fbc 100644 --- a/system/component_state_monitor/config/topics.yaml +++ b/system/component_state_monitor/config/topics.yaml @@ -59,7 +59,7 @@ topic_type: geometry_msgs/msg/PoseWithCovarianceStamped best_effort: false transient_local: false - warn_rate: 5.0 + warn_rate: 2.0 error_rate: 1.0 timeout: 1.0 diff --git a/system/diagnostic_graph_aggregator/CMakeLists.txt b/system/diagnostic_graph_aggregator/CMakeLists.txt index c4a67032541b2..574978ec5adce 100644 --- a/system/diagnostic_graph_aggregator/CMakeLists.txt +++ b/system/diagnostic_graph_aggregator/CMakeLists.txt @@ -22,10 +22,17 @@ ament_auto_add_executable(converter ) target_include_directories(converter PRIVATE src/common) -ament_auto_add_executable(tool - src/tool/tool.cpp +ament_auto_add_executable(tree + src/tool/tree.cpp + src/tool/utils/loader.cpp ) -target_include_directories(tool PRIVATE src/common) +target_include_directories(tree PRIVATE src/common) + +ament_auto_add_executable(plantuml + src/tool/plantuml.cpp + src/tool/utils/loader.cpp +) +target_include_directories(plantuml PRIVATE src/common) if(BUILD_TESTING) get_filename_component(RESOURCE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/test/files ABSOLUTE) diff --git a/system/diagnostic_graph_aggregator/README.md b/system/diagnostic_graph_aggregator/README.md index 4dc44feaafc33..cc49ed88ba98f 100644 --- a/system/diagnostic_graph_aggregator/README.md +++ b/system/diagnostic_graph_aggregator/README.md @@ -53,20 +53,33 @@ This feature breaks the generality of the graph and may be changed to a plugin o ## Examples -- [example_0.yaml](./example/example_0.yaml) -- [example_1.yaml](./example/example_1.yaml) -- [example_2.yaml](./example/example_2.yaml) +This is an example of a diagnostic graph configuration. The configuration can be split into multiple files. + +- [main.yaml](./example/graph/main.yaml) +- [module1.yaml](./example/graph/module1.yaml) +- [module2.yaml](./example/graph/module2.yaml) + +```bash +ros2 launch diagnostic_graph_aggregator example-main.launch.xml +``` + +You can reuse the graph by making partial edits. For example, disable hardware checks for simulation. + +- [edit.yaml](./example/graph/edit.yaml) ```bash -ros2 launch diagnostic_graph_aggregator example.launch.xml +ros2 launch diagnostic_graph_aggregator example-edit.launch.xml ``` +## Debug tools + +- [dump](./doc/tool/dump.md) +- [converter](./doc/tool/converter.md) +- [tree](./doc/tool/tree.md) + ## Graph file format -- [GraphFile](./doc/format/graph-file.md) -- [Path](./doc/format/path.md) -- [Node](./doc/format/node.md) - - [Diag](./doc/format/diag.md) - - [Unit](./doc/format/unit.md) - - [And](./doc/format/and.md) - - [Or](./doc/format/or.md) +- [graph](./doc/format/graph.md) +- [path](./doc/format/path.md) +- [node](./doc/format/node.md) +- [edit](./doc/format/edit.md) diff --git a/system/diagnostic_graph_aggregator/doc/format/and.md b/system/diagnostic_graph_aggregator/doc/format/and.md deleted file mode 100644 index a92aa2817832e..0000000000000 --- a/system/diagnostic_graph_aggregator/doc/format/and.md +++ /dev/null @@ -1,11 +0,0 @@ -# Unit - -And is a node that is evaluated as the AND of the input nodes. - -## Format - -| Name | Type | Required | Description | -| ---- | ------------------------------------------ | -------- | ------------------------------------- | -| type | string | yes | Specify `and` when using this object. | -| name | string | yes | Name of diagnostic status. | -| list | List<[Diag](./diag.md)\|[Unit](./unit.md)> | yes | List of input node references. | diff --git a/system/diagnostic_graph_aggregator/doc/format/diag.md b/system/diagnostic_graph_aggregator/doc/format/diag.md deleted file mode 100644 index 04e850129cbbf..0000000000000 --- a/system/diagnostic_graph_aggregator/doc/format/diag.md +++ /dev/null @@ -1,10 +0,0 @@ -# Diag - -Diag is a node that refers to a source diagnostics. - -## Format - -| Name | Type | Required | Description | -| ---- | ------ | -------- | -------------------------------------- | -| type | string | yes | Specify `diag` when using this object. | -| diag | string | yes | Name of diagnostic status. | diff --git a/system/diagnostic_graph_aggregator/doc/format/edit.md b/system/diagnostic_graph_aggregator/doc/format/edit.md new file mode 100644 index 0000000000000..71ea8a96bc17f --- /dev/null +++ b/system/diagnostic_graph_aggregator/doc/format/edit.md @@ -0,0 +1,14 @@ +# Edit + +The `edit` is a base object that edits the existing diagnostic graph. +Any derived object can be used where a edit object is required. + +## Format + +| Name | Type | Required | Description | +| ------ | -------- | -------- | ------------------------------------------------- | +| `type` | `string` | yes | The string indicating the type of derived object. | + +## Derived objects + +- [remove](./edit/remove.md) diff --git a/system/diagnostic_graph_aggregator/doc/format/edit/remove.md b/system/diagnostic_graph_aggregator/doc/format/edit/remove.md new file mode 100644 index 0000000000000..6cd4b25e98828 --- /dev/null +++ b/system/diagnostic_graph_aggregator/doc/format/edit/remove.md @@ -0,0 +1,10 @@ +# Remove + +The `remove` object is a edit that removes other nodes. + +## Format + +| Name | Type | Required | Description | +| ------ | -------- | -------- | ---------------------------------------- | +| `type` | `string` | yes | Specify `remove` when using this object. | +| `path` | `string` | yes | The path of the node to remove. | diff --git a/system/diagnostic_graph_aggregator/doc/format/graph-file.md b/system/diagnostic_graph_aggregator/doc/format/graph-file.md deleted file mode 100644 index 3c4cfec996a4a..0000000000000 --- a/system/diagnostic_graph_aggregator/doc/format/graph-file.md +++ /dev/null @@ -1,10 +0,0 @@ -# GraphFile - -GraphFile is the top level object that makes up the configuration file. - -## Format - -| Name | Type | Required | Description | -| ----- | ----------------------- | -------- | ------------------------------ | -| files | List<[Path](./path.md)> | no | Paths of the files to include. | -| nodes | List<[Node](./node.md)> | no | Nodes of the diagnostic graph. | diff --git a/system/diagnostic_graph_aggregator/doc/format/graph.md b/system/diagnostic_graph_aggregator/doc/format/graph.md new file mode 100644 index 0000000000000..9dace8a2f0e6d --- /dev/null +++ b/system/diagnostic_graph_aggregator/doc/format/graph.md @@ -0,0 +1,11 @@ +# Graph + +The graph object is the top level structure that makes up the configuration file. + +## Format + +| Name | Type | Required | Description | +| ------- | -------------------------------------- | -------- | ------------------------------------------------- | +| `files` | list\[[path](./path.md)\] | no | List of path objects for importing subgraphs. | +| `nodes` | list\[[node](./node.md)\] | no | List of node objects that make up the graph. | +| `edits` | list\[[edit](./edit.md)\] | no | List of edit objects to partially edit the graph. | diff --git a/system/diagnostic_graph_aggregator/doc/format/node.md b/system/diagnostic_graph_aggregator/doc/format/node.md index da8e8e57b111f..0f4b52b9b8a72 100644 --- a/system/diagnostic_graph_aggregator/doc/format/node.md +++ b/system/diagnostic_graph_aggregator/doc/format/node.md @@ -1,9 +1,25 @@ # Node -Node is a base object that makes up the diagnostic graph. +The `node` is a base object that makes up the diagnostic graph. +Any derived object can be used where a node object is required. ## Format -| Name | Type | Required | Description | -| ---- | ------ | -------- | ------------------------------------------- | -| type | string | yes | Node type. See derived objects for details. | +| Name | Type | Required | Description | +| ------ | -------- | -------- | ------------------------------------------------- | +| `type` | `string` | yes | The string indicating the type of derived object. | +| `path` | `string` | no | Any string to reference from other nodes. | + +## Derived objects + +- [diag](./node/diag.md) +- [and](./node/and.md) +- [or](./node/or.md) +- [remapping](./node/remap.md) + - warn-to-ok + - warn-to-error +- [constant](./node/const.md) + - ok + - warn + - error + - stale diff --git a/system/diagnostic_graph_aggregator/doc/format/node/and.md b/system/diagnostic_graph_aggregator/doc/format/node/and.md new file mode 100644 index 0000000000000..562018bf0995b --- /dev/null +++ b/system/diagnostic_graph_aggregator/doc/format/node/and.md @@ -0,0 +1,17 @@ +# And + +The `and` object is a node that is evaluated as the maximum error level of the input nodes. +Note that error level `stale` is treated as `error`. + +## Format + +| Name | Type | Required | Description | +| ------ | -------------------------------------- | -------- | ------------------------------------------------------------ | +| `type` | string | yes | Specify `and` or `short-circuit-and` when using this object. | +| `list` | list\[[node](../node.md)] | yes | List of input node objects. | + +## Short-circuit evaluation + +!!! warning + + The`short-circuit-and` is work in progress (WIP). diff --git a/system/diagnostic_graph_aggregator/doc/format/node/const.md b/system/diagnostic_graph_aggregator/doc/format/node/const.md new file mode 100644 index 0000000000000..13495be6cdbda --- /dev/null +++ b/system/diagnostic_graph_aggregator/doc/format/node/const.md @@ -0,0 +1,18 @@ +# Constant + +The constant object is a node with a fixed error level. + +## Format + +| Name | Type | Required | Description | +| ------ | -------- | -------- | ------------------------------------------- | +| `type` | `string` | yes | Specify error level when using this object. | + +## Error levels + +The supported error levels are as follows. + +- `ok` +- `warn` +- `error` +- `stale` diff --git a/system/diagnostic_graph_aggregator/doc/format/node/diag.md b/system/diagnostic_graph_aggregator/doc/format/node/diag.md new file mode 100644 index 0000000000000..beba8ed0df5b4 --- /dev/null +++ b/system/diagnostic_graph_aggregator/doc/format/node/diag.md @@ -0,0 +1,10 @@ +# Diag + +The `diag` object is a node that refers to a specific status within the source diagnostics. + +## Format + +| Name | Type | Required | Description | +| ------ | -------- | -------- | -------------------------------------- | +| `type` | `string` | yes | Specify `diag` when using this object. | +| `diag` | `string` | yes | The name of the diagnostic status. | diff --git a/system/diagnostic_graph_aggregator/doc/format/node/link.md b/system/diagnostic_graph_aggregator/doc/format/node/link.md new file mode 100644 index 0000000000000..c23aa92575e54 --- /dev/null +++ b/system/diagnostic_graph_aggregator/doc/format/node/link.md @@ -0,0 +1,10 @@ +# Link + +The `link` object is a node that refers to other nodes. + +## Format + +| Name | Type | Required | Description | +| ------ | -------- | -------- | -------------------------------------- | +| `type` | `string` | yes | Specify `link` when using this object. | +| `link` | `string` | yes | The path of the node to reference. | diff --git a/system/diagnostic_graph_aggregator/doc/format/node/or.md b/system/diagnostic_graph_aggregator/doc/format/node/or.md new file mode 100644 index 0000000000000..74e94ffd628e3 --- /dev/null +++ b/system/diagnostic_graph_aggregator/doc/format/node/or.md @@ -0,0 +1,11 @@ +# Or + +The `or` object is a node that is evaluated as the minimum error level of the input nodes. +Note that error level `stale` is treated as `error`. + +## Format + +| Name | Type | Required | Description | +| ------ | -------------------------------------- | -------- | ------------------------------------ | +| `type` | string | yes | Specify `or` when using this object. | +| `list` | list\[[node](../node.md)] | yes | List of input node objects. | diff --git a/system/diagnostic_graph_aggregator/doc/format/node/remap.md b/system/diagnostic_graph_aggregator/doc/format/node/remap.md new file mode 100644 index 0000000000000..abf0d11fae12d --- /dev/null +++ b/system/diagnostic_graph_aggregator/doc/format/node/remap.md @@ -0,0 +1,21 @@ +# Constant + +!!! warning + + This object is under development. It may be removed in the future. + +The remapping object is a node that converts error levels. + +## Format + +| Name | Type | Required | Description | +| ------ | -------------------------------------- | -------- | ---------------------------------------------------- | +| `type` | `string` | yes | Specify remapping type when using this object. | +| `list` | list\[[node](../node.md)] | yes | List of input node objects. The list size must be 1. | + +## Remapping types + +The supported remapping types are as follows. + +- `warn-to-ok` +- `warn-to-error` diff --git a/system/diagnostic_graph_aggregator/doc/format/or.md b/system/diagnostic_graph_aggregator/doc/format/or.md deleted file mode 100644 index 3e668b686c9e8..0000000000000 --- a/system/diagnostic_graph_aggregator/doc/format/or.md +++ /dev/null @@ -1,11 +0,0 @@ -# Unit - -Or is a node that is evaluated as the OR of the input nodes. - -## Format - -| Name | Type | Required | Description | -| ---- | ------------------------------------------ | -------- | ------------------------------------ | -| type | string | yes | Specify `or` when using this object. | -| name | string | yes | Name of diagnostic status. | -| list | List<[Diag](./diag.md)\|[Unit](./unit.md)> | yes | List of input node references. | diff --git a/system/diagnostic_graph_aggregator/doc/format/path.md b/system/diagnostic_graph_aggregator/doc/format/path.md index 1f27accefa35a..6ca32a7eaad80 100644 --- a/system/diagnostic_graph_aggregator/doc/format/path.md +++ b/system/diagnostic_graph_aggregator/doc/format/path.md @@ -1,10 +1,19 @@ # Path -Path is an object that indicates the path of the file to include. +The path object specifies the file path of the subgraph to be imported. +The structure of the subgraph file should be [graph object](./graph.md). ## Format -| Name | Type | Required | Description | -| ------- | ------ | -------- | ----------------------------- | -| package | string | yes | Package name. | -| path | string | yes | Relative path in the package. | +| Name | Type | Required | Description | +| ------ | -------- | -------- | ------------------------------ | +| `path` | `string` | yes | The file path of the subgraph. | + +## Substitutions + +File paths can contain substitutions like ROS 2 launch. The supported substitutions are as follows. + +| Substitution | Description | +| ----------------------------- | -------------------------------- | +| `$(dirname)` | The path of this file directory. | +| `$(find-pkg-share )` | The path of the package. | diff --git a/system/diagnostic_graph_aggregator/doc/format/unit.md b/system/diagnostic_graph_aggregator/doc/format/unit.md deleted file mode 100644 index 791689aa2d98a..0000000000000 --- a/system/diagnostic_graph_aggregator/doc/format/unit.md +++ /dev/null @@ -1,10 +0,0 @@ -# Unit - -Diag is a node that refers to a functional unit. - -## Format - -| Name | Type | Required | Description | -| ---- | ------ | -------- | -------------------------------------- | -| type | string | yes | Specify `unit` when using this object. | -| name | string | yes | Name of diagnostic status. | diff --git a/system/diagnostic_graph_aggregator/doc/tool/converter.md b/system/diagnostic_graph_aggregator/doc/tool/converter.md new file mode 100644 index 0000000000000..2351bc1e01054 --- /dev/null +++ b/system/diagnostic_graph_aggregator/doc/tool/converter.md @@ -0,0 +1,29 @@ +# Converter tool + +This tool converts `/diagnostics_graph` to `/diagnostics_agg` so it can be read by tools such as `rqt_runtime_monitor` and `rqt_robot_monitor`. + +## Usage + +```bash +ros2 launch diagnostic_graph_aggregator converter.launch.xml complement:=false +``` + +The `complement` argument specifies whether to add an intermediate path that does not exist. +This means that if the graph contains paths `/A/B` and `/A/B/C/D/E`, the intermediate paths `/A`, `/A/B/C` and `/A/B/C/D` will be added. +This is useful for tree view in `rqt_robot_monitor`. The completed node has an error level of `STALE`. + +## Examples + +```bash +ros2 launch diagnostic_graph_aggregator example-main.launch.xml complement:=false +ros2 run rqt_runtime_monitor rqt_runtime_monitor --ros-args -r diagnostics:=diagnostics_agg +``` + +![rqt_runtime_monitor](./images/rqt_runtime_monitor.png) + +```bash +ros2 launch diagnostic_graph_aggregator example-main.launch.xml complement:=true +ros2 run rqt_robot_monitor rqt_robot_monitor +``` + +![rqt_robot_monitor](./images/rqt_robot_monitor.png) diff --git a/system/diagnostic_graph_aggregator/doc/tool/dump.md b/system/diagnostic_graph_aggregator/doc/tool/dump.md new file mode 100644 index 0000000000000..33f05ff866603 --- /dev/null +++ b/system/diagnostic_graph_aggregator/doc/tool/dump.md @@ -0,0 +1,36 @@ +# Dump tool + +This tool displays `/diagnostics_graph` in table format. + +## Usage + +```bash +ros2 run diagnostic_graph_aggregator dump +``` + +## Examples + +```bash +ros2 launch diagnostic_graph_aggregator example-main.launch.xml +ros2 run diagnostic_graph_aggregator dump +``` + +```txt +| ----- | ----- | -------------------------------- | ----- | +| index | level | name | links | +| ----- | ----- | -------------------------------- | ----- | +| 0 | OK | /sensing/radars/front | | +| 1 | OK | /sensing/lidars/front | | +| 2 | ERROR | /sensing/lidars/top | | +| 3 | OK | /functions/obstacle_detection | 1 0 | +| 4 | ERROR | /functions/pose_estimation | 2 | +| 5 | OK | /external/remote_command | | +| 6 | OK | /external/joystick_command | | +| 7 | ERROR | /autoware/modes/pull_over | 4 3 | +| 8 | OK | /autoware/modes/comfortable_stop | 3 | +| 9 | OK | /autoware/modes/emergency_stop | | +| 10 | OK | /autoware/modes/remote | 5 | +| 11 | OK | /autoware/modes/local | 6 | +| 12 | ERROR | /autoware/modes/autonomous | 4 3 | +| 13 | OK | /autoware/modes/stop | | +``` diff --git a/system/diagnostic_graph_aggregator/doc/tool/images/rqt_robot_monitor.png b/system/diagnostic_graph_aggregator/doc/tool/images/rqt_robot_monitor.png new file mode 100644 index 0000000000000..e4aa169f02add Binary files /dev/null and b/system/diagnostic_graph_aggregator/doc/tool/images/rqt_robot_monitor.png differ diff --git a/system/diagnostic_graph_aggregator/doc/tool/images/rqt_runtime_monitor.png b/system/diagnostic_graph_aggregator/doc/tool/images/rqt_runtime_monitor.png new file mode 100644 index 0000000000000..768fee688f594 Binary files /dev/null and b/system/diagnostic_graph_aggregator/doc/tool/images/rqt_runtime_monitor.png differ diff --git a/system/diagnostic_graph_aggregator/doc/tool/tree.md b/system/diagnostic_graph_aggregator/doc/tool/tree.md new file mode 100644 index 0000000000000..79fa61f527038 --- /dev/null +++ b/system/diagnostic_graph_aggregator/doc/tool/tree.md @@ -0,0 +1,40 @@ +# Tree tool + +This tool displays the graph structure of the configuration file in tree format. + +## Usage + +```bash +ros2 run diagnostic_graph_aggregator tree +``` + +## Examples + +```bash +ros2 run diagnostic_graph_aggregator tree system/diagnostic_graph_aggregator/example/graph/main.yaml +``` + +```txt +===== root nodes ================================= +- /autoware/modes/local (and) + - /external/joystick_command (diag) +- /autoware/modes/comfortable_stop (and) + - /functions/obstacle_detection (or) +- /autoware/modes/pull_over (and) + - /functions/pose_estimation (and) + - /functions/obstacle_detection (or) +- /autoware/modes/autonomous (and) + - /functions/pose_estimation (and) + - /functions/obstacle_detection (or) +- /autoware/modes/remote (and) + - /external/remote_command (diag) +===== intermediate nodes ========================= +- /functions/obstacle_detection (or) + - /sensing/lidars/front (diag) + - /sensing/radars/front (diag) +- /functions/pose_estimation (and) + - /sensing/lidars/top (diag) +===== isolated nodes ============================= +- /autoware/modes/stop (const) +- /autoware/modes/emergency_stop (const) +``` diff --git a/system/diagnostic_graph_aggregator/example/example_diags.py b/system/diagnostic_graph_aggregator/example/dummy-diags.py similarity index 93% rename from system/diagnostic_graph_aggregator/example/example_diags.py rename to system/diagnostic_graph_aggregator/example/dummy-diags.py index 52d6189a63f30..08f81bcd738ed 100755 --- a/system/diagnostic_graph_aggregator/example/example_diags.py +++ b/system/diagnostic_graph_aggregator/example/dummy-diags.py @@ -59,6 +59,9 @@ def create_status(name: str): "external_command_checker: joystick_command", "external_command_checker: remote_command", ] - rclpy.init() - rclpy.spin(DummyDiagnostics(diags)) - rclpy.shutdown() + try: + rclpy.init() + rclpy.spin(DummyDiagnostics(diags)) + rclpy.shutdown() + except KeyboardInterrupt: + pass diff --git a/system/diagnostic_graph_aggregator/example/example-edit.launch.xml b/system/diagnostic_graph_aggregator/example/example-edit.launch.xml new file mode 100644 index 0000000000000..c168b76f19c21 --- /dev/null +++ b/system/diagnostic_graph_aggregator/example/example-edit.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/system/diagnostic_graph_aggregator/example/example-main.launch.xml b/system/diagnostic_graph_aggregator/example/example-main.launch.xml new file mode 100644 index 0000000000000..cdc1bc8afca53 --- /dev/null +++ b/system/diagnostic_graph_aggregator/example/example-main.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/system/diagnostic_graph_aggregator/example/example.launch.xml b/system/diagnostic_graph_aggregator/example/example.launch.xml deleted file mode 100644 index 71e59a1833d6e..0000000000000 --- a/system/diagnostic_graph_aggregator/example/example.launch.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/system/diagnostic_graph_aggregator/example/graph/edit.yaml b/system/diagnostic_graph_aggregator/example/graph/edit.yaml new file mode 100644 index 0000000000000..2fbdbffca438d --- /dev/null +++ b/system/diagnostic_graph_aggregator/example/graph/edit.yaml @@ -0,0 +1,6 @@ +files: + - { path: $(find-pkg-share diagnostic_graph_aggregator)/example/graph/main.yaml } + +edits: + - path: /functions/obstacle_detection + type: remove diff --git a/system/diagnostic_graph_aggregator/example/example_0.yaml b/system/diagnostic_graph_aggregator/example/graph/main.yaml similarity index 83% rename from system/diagnostic_graph_aggregator/example/example_0.yaml rename to system/diagnostic_graph_aggregator/example/graph/main.yaml index 7b01883723c76..2bea907d9c119 100644 --- a/system/diagnostic_graph_aggregator/example/example_0.yaml +++ b/system/diagnostic_graph_aggregator/example/graph/main.yaml @@ -1,6 +1,6 @@ files: - - { path: $(find-pkg-share diagnostic_graph_aggregator)/example/example_1.yaml } - - { path: $(find-pkg-share diagnostic_graph_aggregator)/example/example_2.yaml } + - { path: $(dirname)/module1.yaml } + - { path: $(dirname)/module2.yaml } nodes: - path: /autoware/modes/stop diff --git a/system/diagnostic_graph_aggregator/example/example_1.yaml b/system/diagnostic_graph_aggregator/example/graph/module1.yaml similarity index 100% rename from system/diagnostic_graph_aggregator/example/example_1.yaml rename to system/diagnostic_graph_aggregator/example/graph/module1.yaml diff --git a/system/diagnostic_graph_aggregator/example/example_2.yaml b/system/diagnostic_graph_aggregator/example/graph/module2.yaml similarity index 100% rename from system/diagnostic_graph_aggregator/example/example_2.yaml rename to system/diagnostic_graph_aggregator/example/graph/module2.yaml diff --git a/system/diagnostic_graph_aggregator/launch/diagnostic_graph_aggregator.launch.xml b/system/diagnostic_graph_aggregator/launch/aggregator.launch.xml similarity index 100% rename from system/diagnostic_graph_aggregator/launch/diagnostic_graph_aggregator.launch.xml rename to system/diagnostic_graph_aggregator/launch/aggregator.launch.xml diff --git a/system/diagnostic_graph_aggregator/launch/converter.launch.xml b/system/diagnostic_graph_aggregator/launch/converter.launch.xml new file mode 100644 index 0000000000000..9111338bf622a --- /dev/null +++ b/system/diagnostic_graph_aggregator/launch/converter.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/system/diagnostic_graph_aggregator/script/dump.py b/system/diagnostic_graph_aggregator/script/dump.py index 3e639cbce104d..9abcaeb7a080c 100755 --- a/system/diagnostic_graph_aggregator/script/dump.py +++ b/system/diagnostic_graph_aggregator/script/dump.py @@ -84,7 +84,6 @@ def callback(self, msg): if __name__ == "__main__": parser = argparse.ArgumentParser() parser.add_argument("--topic", default="/diagnostics_graph") - parser.add_argument("--order", default="index") args, unparsed = parser.parse_known_args() try: diff --git a/system/diagnostic_graph_aggregator/src/node/aggregator.cpp b/system/diagnostic_graph_aggregator/src/node/aggregator.cpp index 48f489c186bdb..cd16ce8e33e59 100644 --- a/system/diagnostic_graph_aggregator/src/node/aggregator.cpp +++ b/system/diagnostic_graph_aggregator/src/node/aggregator.cpp @@ -20,7 +20,7 @@ namespace diagnostic_graph_aggregator { -MainNode::MainNode() : Node("diagnostic_graph_aggregator_aggregator") +AggregatorNode::AggregatorNode() : Node("aggregator") { // Init diagnostics graph. { @@ -39,7 +39,7 @@ MainNode::MainNode() : Node("diagnostic_graph_aggregator_aggregator") const auto qos_input = rclcpp::QoS(declare_parameter("input_qos_depth")); const auto qos_graph = rclcpp::QoS(declare_parameter("graph_qos_depth")); - const auto callback = std::bind(&MainNode::on_diag, this, _1); + const auto callback = std::bind(&AggregatorNode::on_diag, this, _1); sub_input_ = create_subscription("/diagnostics", qos_input, callback); pub_graph_ = create_publisher("/diagnostics_graph", qos_graph); @@ -51,12 +51,12 @@ MainNode::MainNode() : Node("diagnostic_graph_aggregator_aggregator") debug_ = declare_parameter("use_debug_mode"); } -MainNode::~MainNode() +AggregatorNode::~AggregatorNode() { // for unique_ptr } -void MainNode::on_timer() +void AggregatorNode::on_timer() { const auto stamp = now(); pub_graph_->publish(graph_.report(stamp)); @@ -64,7 +64,7 @@ void MainNode::on_timer() if (modes_) modes_->update(stamp); } -void MainNode::on_diag(const DiagnosticArray::ConstSharedPtr msg) +void AggregatorNode::on_diag(const DiagnosticArray::ConstSharedPtr msg) { graph_.callback(now(), *msg); } @@ -73,10 +73,10 @@ void MainNode::on_diag(const DiagnosticArray::ConstSharedPtr msg) int main(int argc, char ** argv) { - using diagnostic_graph_aggregator::MainNode; + using diagnostic_graph_aggregator::AggregatorNode; rclcpp::init(argc, argv); rclcpp::executors::SingleThreadedExecutor executor; - auto node = std::make_shared(); + auto node = std::make_shared(); executor.add_node(node); executor.spin(); executor.remove_node(node); diff --git a/system/diagnostic_graph_aggregator/src/node/aggregator.hpp b/system/diagnostic_graph_aggregator/src/node/aggregator.hpp index dd64809e939cc..6bf9aead9754d 100644 --- a/system/diagnostic_graph_aggregator/src/node/aggregator.hpp +++ b/system/diagnostic_graph_aggregator/src/node/aggregator.hpp @@ -26,11 +26,11 @@ namespace diagnostic_graph_aggregator { -class MainNode : public rclcpp::Node +class AggregatorNode : public rclcpp::Node { public: - MainNode(); - ~MainNode(); + AggregatorNode(); + ~AggregatorNode(); private: Graph graph_; diff --git a/system/diagnostic_graph_aggregator/src/node/converter.cpp b/system/diagnostic_graph_aggregator/src/node/converter.cpp index 89acfce31a5ab..f0b2108374017 100644 --- a/system/diagnostic_graph_aggregator/src/node/converter.cpp +++ b/system/diagnostic_graph_aggregator/src/node/converter.cpp @@ -14,8 +14,7 @@ #include "converter.hpp" -#include -#include +#include namespace diagnostic_graph_aggregator { @@ -35,18 +34,46 @@ std::string level_to_string(DiagnosticLevel level) return "UNKNOWN"; } -ToolNode::ToolNode() : Node("diagnostic_graph_aggregator_converter") +std::string parent_path(const std::string & path) +{ + return path.substr(0, path.rfind('/')); +} + +auto create_tree(const DiagnosticGraph & graph) +{ + std::map, std::greater> tree; + for (const auto & node : graph.nodes) { + tree.emplace(node.status.name, std::make_unique(true)); + } + for (const auto & node : graph.nodes) { + std::string path = node.status.name; + while (path = parent_path(path), !path.empty()) { + if (tree.count(path)) break; + tree.emplace(path, std::make_unique(false)); + } + } + for (const auto & [path, node] : tree) { + const auto parent = parent_path(path); + node->parent = parent.empty() ? nullptr : tree[parent].get(); + } + return tree; +} + +ConverterNode::ConverterNode() : Node("converter") { using std::placeholders::_1; const auto qos_graph = rclcpp::QoS(1); const auto qos_array = rclcpp::QoS(1); - const auto callback = std::bind(&ToolNode::on_graph, this, _1); + const auto callback = std::bind(&ConverterNode::on_graph, this, _1); sub_graph_ = create_subscription("/diagnostics_graph", qos_graph, callback); - pub_array_ = create_publisher("/diagnostics_array", qos_array); + pub_array_ = create_publisher("/diagnostics_agg", qos_array); + + initialize_tree_ = false; + complement_tree_ = declare_parameter("complement_tree"); } -void ToolNode::on_graph(const DiagnosticGraph::ConstSharedPtr msg) +void ConverterNode::on_graph(const DiagnosticGraph::ConstSharedPtr msg) { DiagnosticArray message; message.header.stamp = msg->stamp; @@ -63,6 +90,31 @@ void ToolNode::on_graph(const DiagnosticGraph::ConstSharedPtr msg) } } } + + if (complement_tree_ && !initialize_tree_) { + initialize_tree_ = true; + tree_ = create_tree(*msg); + } + + if (complement_tree_) { + for (const auto & [path, node] : tree_) { + node->level = DiagnosticStatus::OK; + } + for (const auto & node : msg->nodes) { + tree_[node.status.name]->level = node.status.level; + } + for (const auto & [path, node] : tree_) { + if (!node->parent) continue; + node->parent->level = std::max(node->parent->level, node->level); + } + for (const auto & [path, node] : tree_) { + if (node->leaf) continue; + message.status.emplace_back(); + message.status.back().name = path; + message.status.back().level = node->level; + } + } + pub_array_->publish(message); } @@ -70,10 +122,10 @@ void ToolNode::on_graph(const DiagnosticGraph::ConstSharedPtr msg) int main(int argc, char ** argv) { - using diagnostic_graph_aggregator::ToolNode; + using diagnostic_graph_aggregator::ConverterNode; rclcpp::init(argc, argv); rclcpp::executors::SingleThreadedExecutor executor; - auto node = std::make_shared(); + auto node = std::make_shared(); executor.add_node(node); executor.spin(); executor.remove_node(node); diff --git a/system/diagnostic_graph_aggregator/src/node/converter.hpp b/system/diagnostic_graph_aggregator/src/node/converter.hpp index 7a3e8e4e58033..1f98abed4d619 100644 --- a/system/diagnostic_graph_aggregator/src/node/converter.hpp +++ b/system/diagnostic_graph_aggregator/src/node/converter.hpp @@ -19,15 +19,32 @@ #include +#include +#include // Use map for sorting keys. +#include +#include +#include + namespace diagnostic_graph_aggregator { -class ToolNode : public rclcpp::Node +struct TreeNode +{ + explicit TreeNode(bool leaf) : leaf(leaf) {} + bool leaf; + TreeNode * parent; + uint8_t level; +}; + +class ConverterNode : public rclcpp::Node { public: - ToolNode(); + ConverterNode(); private: + bool initialize_tree_; + bool complement_tree_; + std::map, std::greater> tree_; rclcpp::Subscription::SharedPtr sub_graph_; rclcpp::Publisher::SharedPtr pub_array_; void on_graph(const DiagnosticGraph::ConstSharedPtr msg); diff --git a/system/diagnostic_graph_aggregator/src/tool/plantuml.cpp b/system/diagnostic_graph_aggregator/src/tool/plantuml.cpp new file mode 100644 index 0000000000000..bfbb382a78fcb --- /dev/null +++ b/system/diagnostic_graph_aggregator/src/tool/plantuml.cpp @@ -0,0 +1,49 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "utils/loader.hpp" + +#include + +namespace diagnostic_graph_aggregator +{ + +void dump_root(const std::string & path) +{ + const auto graph = load_graph_nodes(path); + const auto color = "#FFFFFF"; + + for (const auto & node : graph.nodes) { + std::cout << "card " << node << " " << color << " [" << std::endl; + std::cout << node->path << std::endl; + std::cout << "]" << std::endl; + } + + for (const auto & node : graph.nodes) { + for (const auto & child : node->children) { + std::cout << node << " --> " << child << std::endl; + } + } +} + +} // namespace diagnostic_graph_aggregator + +int main(int argc, char ** argv) +{ + if (argc != 2) { + std::cerr << "usage: plantuml " << std::endl; + return 1; + } + diagnostic_graph_aggregator::dump_root(argv[1]); +} diff --git a/system/diagnostic_graph_aggregator/src/tool/tool.cpp b/system/diagnostic_graph_aggregator/src/tool/tool.cpp deleted file mode 100644 index 657f76c2d91b7..0000000000000 --- a/system/diagnostic_graph_aggregator/src/tool/tool.cpp +++ /dev/null @@ -1,140 +0,0 @@ -// Copyright 2023 The Autoware Contributors -// -// 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 "graph/graph.hpp" -#include "graph/types.hpp" -#include "graph/units.hpp" - -#include -#include -#include - -namespace diagnostic_graph_aggregator -{ - -struct GraphNode -{ - using UniquePtr = std::unique_ptr; - std::string type; - std::string path; - std::vector children; - std::vector parents; -}; - -struct GraphRoot -{ - std::vector owner; - std::vector nodes; -}; - -GraphRoot load_graph_nodes(const std::string & path) -{ - GraphRoot result; - { - std::unordered_map mapping; - Graph graph; - graph.init(path); - - for (const auto & node : graph.nodes()) { - auto data = std::make_unique(); - data->path = node->path(); - data->type = node->type(); - mapping[node] = std::move(data); - } - - for (const auto & [node, data] : mapping) { - for (const auto & link : node->children()) { - const auto parent = data.get(); - const auto child = mapping.at(link).get(); - child->parents.push_back(parent); - parent->children.push_back(child); - } - } - - for (auto & [node, data] : mapping) { - result.owner.push_back(std::move(data)); - } - for (const auto & node : result.owner) { - result.nodes.push_back(node.get()); - } - } - return result; -} - -void dump_plantuml_path(const std::string & path) -{ - const auto graph = load_graph_nodes(path); - const auto color = "#FFFFFF"; - - for (const auto & node : graph.nodes) { - std::cout << "card " << node << " " << color << " [" << std::endl; - std::cout << node->path << std::endl; - std::cout << "]" << std::endl; - } - - for (const auto & node : graph.nodes) { - for (const auto & child : node->children) { - std::cout << node << " --> " << child << std::endl; - } - } -} - -void dump_tree_node(const GraphNode * node, const std::string & indent = "", bool root = true) -{ - const auto path = node->path.empty() ? "" : node->path + " "; - const auto type = "(" + node->type + ")"; - std::cout << indent << "- " << path << type << std::endl; - - if (root || node->parents.size() == 1) { - for (const auto child : node->children) { - dump_tree_node(child, indent + " ", false); - } - } -} - -void dump_tree_path(const std::string & path) -{ - const auto graph = load_graph_nodes(path); - - std::cout << "===== root nodes =================================" << std::endl; - for (const auto & node : graph.nodes) { - if (node->parents.size() == 0 && node->children.size() != 0) { - dump_tree_node(node); - } - } - std::cout << "===== intermediate nodes =========================" << std::endl; - for (const auto & node : graph.nodes) { - if (node->parents.size() >= 2) { - dump_tree_node(node); - } - } - - std::cout << "===== isolated nodes =============================" << std::endl; - for (const auto & node : graph.nodes) { - if (node->parents.size() == 0 && node->children.size() == 0) { - dump_tree_node(node); - } - } -} - -} // namespace diagnostic_graph_aggregator - -int main(int argc, char ** argv) -{ - if (argc != 2) { - std::cerr << "usage: " << argv[0] << " " << std::endl; - return 1; - } - diagnostic_graph_aggregator::dump_tree_path(argv[1]); -} diff --git a/system/diagnostic_graph_aggregator/src/tool/tree.cpp b/system/diagnostic_graph_aggregator/src/tool/tree.cpp new file mode 100644 index 0000000000000..9dbbc539b3b91 --- /dev/null +++ b/system/diagnostic_graph_aggregator/src/tool/tree.cpp @@ -0,0 +1,69 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "utils/loader.hpp" + +#include + +namespace diagnostic_graph_aggregator +{ + +void dump_node(const GraphNode * node, const std::string & indent = "", bool root = true) +{ + const auto path = node->path.empty() ? "" : node->path + " "; + const auto type = "(" + node->type + ")"; + std::cout << indent << "- " << path << type << std::endl; + + if (root || node->parents.size() == 1) { + for (const auto child : node->children) { + dump_node(child, indent + " ", false); + } + } +} + +void dump_root(const std::string & path) +{ + const auto graph = load_graph_nodes(path); + + std::cout << "===== root nodes =================================" << std::endl; + for (const auto & node : graph.nodes) { + if (node->parents.size() == 0 && node->children.size() != 0) { + dump_node(node); + } + } + std::cout << "===== intermediate nodes =========================" << std::endl; + for (const auto & node : graph.nodes) { + if (node->parents.size() >= 2) { + dump_node(node); + } + } + + std::cout << "===== isolated nodes =============================" << std::endl; + for (const auto & node : graph.nodes) { + if (node->parents.size() == 0 && node->children.size() == 0) { + dump_node(node); + } + } +} + +} // namespace diagnostic_graph_aggregator + +int main(int argc, char ** argv) +{ + if (argc != 2) { + std::cerr << "usage: tree " << std::endl; + return 1; + } + diagnostic_graph_aggregator::dump_root(argv[1]); +} diff --git a/system/diagnostic_graph_aggregator/src/tool/utils/loader.cpp b/system/diagnostic_graph_aggregator/src/tool/utils/loader.cpp new file mode 100644 index 0000000000000..a2e6ce8b85334 --- /dev/null +++ b/system/diagnostic_graph_aggregator/src/tool/utils/loader.cpp @@ -0,0 +1,57 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "loader.hpp" + +#include +#include + +namespace diagnostic_graph_aggregator +{ + +GraphRoot load_graph_nodes(const std::string & path) +{ + GraphRoot result; + { + std::unordered_map mapping; + Graph graph; + graph.init(path); + + for (const auto & node : graph.nodes()) { + auto data = std::make_unique(); + data->path = node->path(); + data->type = node->type(); + mapping[node] = std::move(data); + } + + for (const auto & [node, data] : mapping) { + for (const auto & link : node->children()) { + const auto parent = data.get(); + const auto child = mapping.at(link).get(); + child->parents.push_back(parent); + parent->children.push_back(child); + } + } + + for (auto & [node, data] : mapping) { + result.owner.push_back(std::move(data)); + } + for (const auto & node : result.owner) { + result.nodes.push_back(node.get()); + } + } + return result; +} + +} // namespace diagnostic_graph_aggregator diff --git a/system/diagnostic_graph_aggregator/src/tool/utils/loader.hpp b/system/diagnostic_graph_aggregator/src/tool/utils/loader.hpp new file mode 100644 index 0000000000000..70e01ec2a77b4 --- /dev/null +++ b/system/diagnostic_graph_aggregator/src/tool/utils/loader.hpp @@ -0,0 +1,48 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 TOOL__UTILS__LOADER_HPP_ +#define TOOL__UTILS__LOADER_HPP_ + +#include "graph/graph.hpp" +#include "graph/types.hpp" +#include "graph/units.hpp" + +#include +#include +#include + +namespace diagnostic_graph_aggregator +{ + +struct GraphNode +{ + using UniquePtr = std::unique_ptr; + std::string type; + std::string path; + std::vector children; + std::vector parents; +}; + +struct GraphRoot +{ + std::vector owner; + std::vector nodes; +}; + +GraphRoot load_graph_nodes(const std::string & path); + +} // namespace diagnostic_graph_aggregator + +#endif // TOOL__UTILS__LOADER_HPP_ diff --git a/system/emergency_handler/config/emergency_handler.param.yaml b/system/emergency_handler/config/emergency_handler.param.yaml index 652a984ce539a..2309e7f23deb0 100644 --- a/system/emergency_handler/config/emergency_handler.param.yaml +++ b/system/emergency_handler/config/emergency_handler.param.yaml @@ -4,8 +4,6 @@ ros__parameters: update_rate: 10 timeout_hazard_status: 0.5 - timeout_takeover_request: 10.0 - use_takeover_request: false use_parking_after_stopped: false use_comfortable_stop: false diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp index 6d83c6781fad7..5c76d96e573ab 100644 --- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp +++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp @@ -45,8 +45,6 @@ struct Param { int update_rate; double timeout_hazard_status; - double timeout_takeover_request; - bool use_takeover_request; bool use_parking_after_stopped; bool use_comfortable_stop; HazardLampPolicy turning_hazard_on{}; @@ -133,16 +131,16 @@ class EmergencyHandler : public rclcpp::Node // Heartbeat rclcpp::Time stamp_hazard_status_; + bool is_hazard_status_timeout_; + void checkHazardStatusTimeout(); // Algorithm - rclcpp::Time takeover_requested_time_; - bool is_takeover_request_ = false; void transitionTo(const int new_state); void updateMrmState(); void operateMrm(); autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); bool isStopped(); - bool isEmergency(const autoware_auto_system_msgs::msg::HazardStatus & hazard_status); + bool isEmergency(); }; #endif // EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_ diff --git a/system/emergency_handler/schema/emergency_handler.schema.json b/system/emergency_handler/schema/emergency_handler.schema.json index 8295a5c6efb6b..3276ae3fa1553 100644 --- a/system/emergency_handler/schema/emergency_handler.schema.json +++ b/system/emergency_handler/schema/emergency_handler.schema.json @@ -16,16 +16,6 @@ "description": "If the input `hazard_status` topic cannot be received for more than `timeout_hazard_status`, vehicle will make an emergency stop.", "default": 0.5 }, - "timeout_takeover_request": { - "type": "number", - "description": "Transition to MRR_OPERATING if the time from the last takeover request exceeds `timeout_takeover_request`.", - "default": 10.0 - }, - "use_takeover_request": { - "type": "boolean", - "description": "If this parameter is true, the handler will record the time and make take over request to the driver when emergency state occurs.", - "default": "false" - }, "use_parking_after_stopped": { "type": "boolean", "description": "If this parameter is true, it will publish PARKING shift command.", @@ -51,8 +41,6 @@ "required": [ "update_rate", "timeout_hazard_status", - "timeout_takeover_request", - "use_takeover_request", "use_parking_after_stopped", "use_comfortable_stop", "turning_hazard_on" diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp index 17fc92f3d36c7..db3ad40249bfb 100644 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp +++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp @@ -23,8 +23,6 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler") // Parameter param_.update_rate = declare_parameter("update_rate"); param_.timeout_hazard_status = declare_parameter("timeout_hazard_status"); - param_.timeout_takeover_request = declare_parameter("timeout_takeover_request"); - param_.use_takeover_request = declare_parameter("use_takeover_request"); param_.use_parking_after_stopped = declare_parameter("use_parking_after_stopped"); param_.use_comfortable_stop = declare_parameter("use_comfortable_stop"); param_.turning_hazard_on.emergency = declare_parameter("turning_hazard_on.emergency"); @@ -85,6 +83,7 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler") mrm_state_.stamp = this->now(); mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL; mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE; + is_hazard_status_timeout_ = false; // Timer const auto update_period_ns = rclcpp::Rate(param_.update_rate).period(); @@ -137,7 +136,7 @@ autoware_auto_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHaz HazardLightsCommand msg; // Check emergency - const bool is_emergency = isEmergency(hazard_status_stamped_->status); + const bool is_emergency = isEmergency(); if (hazard_status_stamped_->status.emergency_holding) { // turn hazard on during emergency holding @@ -311,22 +310,27 @@ bool EmergencyHandler::isDataReady() return true; } -void EmergencyHandler::onTimer() +void EmergencyHandler::checkHazardStatusTimeout() { - if (!isDataReady()) { - return; - } - const bool is_hazard_status_timeout = - (this->now() - stamp_hazard_status_).seconds() > param_.timeout_hazard_status; - if (is_hazard_status_timeout) { + if ((this->now() - stamp_hazard_status_).seconds() > param_.timeout_hazard_status) { + is_hazard_status_timeout_ = true; RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), "heartbeat_hazard_status is timeout"); - mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING; - publishControlCommands(); + } else { + is_hazard_status_timeout_ = false; + } +} + +void EmergencyHandler::onTimer() +{ + if (!isDataReady()) { return; } + // Check whether heartbeat hazard_status is timeout + checkHazardStatusTimeout(); + // Update Emergency State updateMrmState(); @@ -371,45 +375,27 @@ void EmergencyHandler::updateMrmState() using autoware_auto_vehicle_msgs::msg::ControlModeReport; // Check emergency - const bool is_emergency = isEmergency(hazard_status_stamped_->status); + const bool is_emergency = isEmergency(); // Get mode const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS; - const bool is_takeover_done = control_mode_->mode == ControlModeReport::MANUAL; // State Machine if (mrm_state_.state == MrmState::NORMAL) { // NORMAL if (is_auto_mode && is_emergency) { - if (param_.use_takeover_request) { - takeover_requested_time_ = this->get_clock()->now(); - is_takeover_request_ = true; - return; - } else { - transitionTo(MrmState::MRM_OPERATING); - return; - } + transitionTo(MrmState::MRM_OPERATING); + return; } } else { // Emergency - // Send recovery events if "not emergency" or "takeover done" + // Send recovery events if "not emergency" if (!is_emergency) { transitionTo(MrmState::NORMAL); return; } - // TODO(Kenji Miyake): Check if human can safely override, for example using DSM - if (is_takeover_done) { - transitionTo(MrmState::NORMAL); - return; - } - if (is_takeover_request_) { - const auto time_from_takeover_request = this->get_clock()->now() - takeover_requested_time_; - if (time_from_takeover_request.seconds() > param_.timeout_takeover_request) { - transitionTo(MrmState::MRM_OPERATING); - return; - } - } else if (mrm_state_.state == MrmState::MRM_OPERATING) { + if (mrm_state_.state == MrmState::MRM_OPERATING) { // TODO(Kenji Miyake): Check MRC is accomplished if (isStopped()) { transitionTo(MrmState::MRM_SUCCEEDED); @@ -432,7 +418,10 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type EmergencyHandler::getCurre using autoware_auto_system_msgs::msg::HazardStatus; // Get hazard level - const auto level = hazard_status_stamped_->status.level; + auto level = hazard_status_stamped_->status.level; + if (is_hazard_status_timeout_) { + level = HazardStatus::SINGLE_POINT_FAULT; + } // State machine if (mrm_state_.behavior == MrmState::NONE) { @@ -455,10 +444,10 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type EmergencyHandler::getCurre return mrm_state_.behavior; } -bool EmergencyHandler::isEmergency( - const autoware_auto_system_msgs::msg::HazardStatus & hazard_status) +bool EmergencyHandler::isEmergency() { - return hazard_status.emergency || hazard_status.emergency_holding; + return hazard_status_stamped_->status.emergency || + hazard_status_stamped_->status.emergency_holding || is_hazard_status_timeout_; } bool EmergencyHandler::isStopped() diff --git a/system/mrm_handler/config/mrm_handler.param.yaml b/system/mrm_handler/config/mrm_handler.param.yaml index 292d459f69d88..e82ee36a7825a 100644 --- a/system/mrm_handler/config/mrm_handler.param.yaml +++ b/system/mrm_handler/config/mrm_handler.param.yaml @@ -4,10 +4,10 @@ ros__parameters: update_rate: 10 timeout_operation_mode_availability: 0.5 + timeout_call_mrm_behavior: 0.01 + timeout_cancel_mrm_behavior: 0.01 use_emergency_holding: false timeout_emergency_recovery: 5.0 - timeout_takeover_request: 10.0 - use_takeover_request: false use_parking_after_stopped: false use_pull_over: false use_comfortable_stop: false diff --git a/system/mrm_handler/image/mrm-state.svg b/system/mrm_handler/image/mrm-state.svg index 13a2956b6f15c..dbcc13918feff 100644 --- a/system/mrm_handler/image/mrm-state.svg +++ b/system/mrm_handler/image/mrm-state.svg @@ -8,302 +8,300 @@ width="958px" height="341px" viewBox="-0.5 -0.5 958 341" - content="<mxfile host="app.diagrams.net" modified="2024-02-07T09:48:58.193Z" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/118.0.0.0 Safari/537.36" etag="JzfBIbSx-UshJtjtpS4o" version="23.1.1" type="device"> <diagram id="C5RBs43oDa-KdzZeNtuy" name="Page-1"> <mxGraphModel dx="1364" dy="771" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="1169" pageHeight="827" math="0" shadow="0"> <root> <mxCell id="WIyWlLk6GJQsqaUBKTNV-0" /> <mxCell id="WIyWlLk6GJQsqaUBKTNV-1" parent="WIyWlLk6GJQsqaUBKTNV-0" /> <mxCell id="6FWRLcRsoSDiZ6vq709X-284" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-281" target="6FWRLcRsoSDiZ6vq709X-3"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="230" y="360" /> <mxPoint x="230" y="360" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-285" value="not emergency" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-284"> <mxGeometry x="0.02" relative="1" as="geometry"> <mxPoint x="1" y="-10" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-281" value="" style="rounded=0;whiteSpace=wrap;html=1;" vertex="1" parent="WIyWlLk6GJQsqaUBKTNV-1"> <mxGeometry x="280" y="140" width="720" height="340" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-4" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;" edge="1" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-2" target="6FWRLcRsoSDiZ6vq709X-3"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-2" value="" style="ellipse;whiteSpace=wrap;html=1;fillColor=#000000;" vertex="1" parent="WIyWlLk6GJQsqaUBKTNV-1"> <mxGeometry x="50" y="305" width="30" height="30" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-19" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.25;exitDx=0;exitDy=0;" edge="1" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-3" target="6FWRLcRsoSDiZ6vq709X-281"> <mxGeometry relative="1" as="geometry"> <mxPoint x="270" y="290" as="targetPoint" /> <Array as="points"> <mxPoint x="260" y="290" /> <mxPoint x="260" y="290" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-21" value="emergency" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-19"> <mxGeometry x="-0.0719" y="-3" relative="1" as="geometry"> <mxPoint y="-13" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-3" value="&lt;b&gt;NORMAL&lt;/b&gt;" style="rounded=0;whiteSpace=wrap;html=1;" vertex="1" parent="WIyWlLk6GJQsqaUBKTNV-1"> <mxGeometry x="120" y="260" width="60" height="120" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-274" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-5" target="6FWRLcRsoSDiZ6vq709X-272"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="810" y="260" /> <mxPoint x="810" y="260" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-275" value="succeeded" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-274"> <mxGeometry x="-0.0667" y="-3" relative="1" as="geometry"> <mxPoint y="-13" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-278" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-5" target="6FWRLcRsoSDiZ6vq709X-273"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="810" y="400" /> <mxPoint x="810" y="400" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-279" value="failed" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-278"> <mxGeometry x="0.2222" y="-3" relative="1" as="geometry"> <mxPoint x="-15" y="-13" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-5" value="MRM_OPERATING" style="swimlane;whiteSpace=wrap;html=1;gradientColor=none;swimlaneFillColor=default;fillColor=default;" vertex="1" parent="WIyWlLk6GJQsqaUBKTNV-1"> <mxGeometry x="310" y="160" width="460" height="300" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-23" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-12" target="6FWRLcRsoSDiZ6vq709X-14"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-31" value="pull over unavailable&lt;br&gt;comfortable_stop available (ca)" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-23"> <mxGeometry x="-0.12" y="-1" relative="1" as="geometry"> <mxPoint x="9" y="-24" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-24" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;entryX=0;entryY=0.5;entryDx=0;entryDy=0;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-12" target="6FWRLcRsoSDiZ6vq709X-13"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="35" y="80" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-26" value="pull over available (pa)" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-24"> <mxGeometry x="0.1529" y="-1" relative="1" as="geometry"> <mxPoint x="13" y="-11" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-25" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-12" target="6FWRLcRsoSDiZ6vq709X-17"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="35" y="240" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-33" value="pull over unavailable&lt;br style=&quot;border-color: var(--border-color);&quot;&gt;comfortable_stop unavailable&lt;br&gt;emergency_stop available (ea)" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-25"> <mxGeometry x="0.3217" relative="1" as="geometry"> <mxPoint x="-4" y="-26" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-12" value="" style="ellipse;whiteSpace=wrap;html=1;fillColor=#000000;" vertex="1" parent="6FWRLcRsoSDiZ6vq709X-5"> <mxGeometry x="20" y="143.75" width="30" height="30" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-27" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-13" target="6FWRLcRsoSDiZ6vq709X-14"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-30" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-13" target="6FWRLcRsoSDiZ6vq709X-17"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="420" y="80" /> <mxPoint x="420" y="240" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-13" value="pull_over" style="rounded=1;whiteSpace=wrap;html=1;" vertex="1" parent="6FWRLcRsoSDiZ6vq709X-5"> <mxGeometry x="240" y="60" width="150" height="40" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-28" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-14" target="6FWRLcRsoSDiZ6vq709X-17"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-152" value="ea" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-28"> <mxGeometry x="-0.1294" y="-1" relative="1" as="geometry"> <mxPoint x="9" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-156" value="ea" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-28"> <mxGeometry x="-0.1294" y="-1" relative="1" as="geometry"> <mxPoint x="116" y="-36" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-15" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-13" target="6FWRLcRsoSDiZ6vq709X-13"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-17" value="emergency_stop" style="rounded=1;whiteSpace=wrap;html=1;" vertex="1" parent="6FWRLcRsoSDiZ6vq709X-5"> <mxGeometry x="240" y="220" width="150" height="37.5" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-32" value="" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-13" target="6FWRLcRsoSDiZ6vq709X-14"> <mxGeometry relative="1" as="geometry"> <mxPoint x="585" y="260" as="sourcePoint" /> <mxPoint x="585" y="380" as="targetPoint" /> <Array as="points" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-150" value="ca" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-32"> <mxGeometry x="-0.1" y="1" relative="1" as="geometry"> <mxPoint x="7" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-14" value="comfortable_stop" style="rounded=1;whiteSpace=wrap;html=1;" vertex="1" parent="6FWRLcRsoSDiZ6vq709X-5"> <mxGeometry x="240" y="140" width="150" height="37.5" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-18" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=1;entryDx=0;entryDy=0;" edge="1" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-2" target="6FWRLcRsoSDiZ6vq709X-2"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-276" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-272" target="6FWRLcRsoSDiZ6vq709X-5"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="780" y="220" /> <mxPoint x="780" y="220" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-277" value="pa" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-276"> <mxGeometry x="-0.1556" y="-2" relative="1" as="geometry"> <mxPoint x="-6" y="-8" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-272" value="&lt;b&gt;MRM_SUCCEEDED&lt;/b&gt;" style="rounded=0;whiteSpace=wrap;html=1;" vertex="1" parent="WIyWlLk6GJQsqaUBKTNV-1"> <mxGeometry x="850" y="200" width="130" height="80" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-273" value="&lt;b&gt;MRM_FAILED&lt;/b&gt;" style="rounded=0;whiteSpace=wrap;html=1;" vertex="1" parent="WIyWlLk6GJQsqaUBKTNV-1"> <mxGeometry x="850" y="350" width="130" height="80" as="geometry" /> </mxCell> </root> </mxGraphModel> </diagram> </mxfile> " + content="<mxfile host="app.diagrams.net" modified="2024-03-01T14:04:38.320Z" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/122.0.0.0 Safari/537.36" etag="jomwCx6bxG3LysB-HIk4" version="23.1.4" type="device"> <diagram id="C5RBs43oDa-KdzZeNtuy" name="Page-1"> <mxGraphModel dx="1364" dy="759" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="1169" pageHeight="827" math="0" shadow="0"> <root> <mxCell id="WIyWlLk6GJQsqaUBKTNV-0" /> <mxCell id="WIyWlLk6GJQsqaUBKTNV-1" parent="WIyWlLk6GJQsqaUBKTNV-0" /> <mxCell id="6FWRLcRsoSDiZ6vq709X-281" value="" style="rounded=0;whiteSpace=wrap;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" vertex="1"> <mxGeometry x="280" y="140" width="720" height="340" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-5" value="MRM_OPERATING" style="swimlane;whiteSpace=wrap;html=1;gradientColor=none;swimlaneFillColor=default;fillColor=default;" parent="WIyWlLk6GJQsqaUBKTNV-1" vertex="1"> <mxGeometry x="310" y="160" width="460" height="300" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-24" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-12" target="6FWRLcRsoSDiZ6vq709X-13" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="35" y="80" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-26" value="pull over available (pa)" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-24" vertex="1" connectable="0"> <mxGeometry x="0.1529" y="-1" relative="1" as="geometry"> <mxPoint x="13" y="-11" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-25" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-12" target="6FWRLcRsoSDiZ6vq709X-17" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="35" y="250" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-33" value="pull over unavailable&lt;br style=&quot;border-color: var(--border-color);&quot;&gt;comfortable_stop unavailable&lt;br&gt;emergency_stop available (ea)" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-25" vertex="1" connectable="0"> <mxGeometry x="0.3217" relative="1" as="geometry"> <mxPoint x="-4" y="-26" as="offset" /> </mxGeometry> </mxCell> <mxCell id="0iophzwyXz64g-DfP8aO-1" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-12" target="6FWRLcRsoSDiZ6vq709X-14"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="0iophzwyXz64g-DfP8aO-2" value="pull over unavailable&lt;br style=&quot;border-color: var(--border-color);&quot;&gt;comfortable_stop available (ca)" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="0iophzwyXz64g-DfP8aO-1"> <mxGeometry x="-0.1263" y="-4" relative="1" as="geometry"> <mxPoint x="7" y="-24" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-12" value="" style="ellipse;whiteSpace=wrap;html=1;fillColor=#000000;" parent="6FWRLcRsoSDiZ6vq709X-5" vertex="1"> <mxGeometry x="20" y="145" width="30" height="30" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-30" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-13" target="6FWRLcRsoSDiZ6vq709X-17" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="420" y="80" /> <mxPoint x="420" y="240" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-13" value="pull_over" style="rounded=1;whiteSpace=wrap;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" vertex="1"> <mxGeometry x="240" y="40" width="150" height="60" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-28" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-14" target="6FWRLcRsoSDiZ6vq709X-17" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="290" y="200" /> <mxPoint x="290" y="200" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-152" value="ea || failed" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-28" vertex="1" connectable="0"> <mxGeometry x="-0.1294" y="-1" relative="1" as="geometry"> <mxPoint x="31" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-156" value="ea || failed" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-28" vertex="1" connectable="0"> <mxGeometry x="-0.1294" y="-1" relative="1" as="geometry"> <mxPoint x="131" y="-36" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-15" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-13" target="6FWRLcRsoSDiZ6vq709X-13" edge="1"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-17" value="emergency_stop" style="rounded=1;whiteSpace=wrap;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" vertex="1"> <mxGeometry x="240" y="220" width="150" height="60" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-32" value="" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-13" target="6FWRLcRsoSDiZ6vq709X-14" edge="1"> <mxGeometry relative="1" as="geometry"> <mxPoint x="585" y="260" as="sourcePoint" /> <mxPoint x="585" y="380" as="targetPoint" /> <Array as="points"> <mxPoint x="290" y="110" /> <mxPoint x="290" y="110" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-150" value="ca" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-32" vertex="1" connectable="0"> <mxGeometry x="-0.1" y="1" relative="1" as="geometry"> <mxPoint x="7" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-14" value="comfortable_stop" style="rounded=1;whiteSpace=wrap;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" vertex="1"> <mxGeometry x="240" y="130" width="150" height="60" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-284" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-281" target="6FWRLcRsoSDiZ6vq709X-3" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="230" y="360" /> <mxPoint x="230" y="360" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-285" value="not emergency" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-284" vertex="1" connectable="0"> <mxGeometry x="0.02" relative="1" as="geometry"> <mxPoint x="1" y="-10" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-4" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-2" target="6FWRLcRsoSDiZ6vq709X-3" edge="1"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-2" value="" style="ellipse;whiteSpace=wrap;html=1;fillColor=#000000;" parent="WIyWlLk6GJQsqaUBKTNV-1" vertex="1"> <mxGeometry x="50" y="305" width="30" height="30" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-19" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.25;exitDx=0;exitDy=0;" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-3" target="6FWRLcRsoSDiZ6vq709X-281" edge="1"> <mxGeometry relative="1" as="geometry"> <mxPoint x="270" y="290" as="targetPoint" /> <Array as="points"> <mxPoint x="260" y="290" /> <mxPoint x="260" y="290" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-21" value="emergency" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-19" vertex="1" connectable="0"> <mxGeometry x="-0.0719" y="-3" relative="1" as="geometry"> <mxPoint y="-13" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-3" value="&lt;b&gt;NORMAL&lt;/b&gt;" style="rounded=0;whiteSpace=wrap;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" vertex="1"> <mxGeometry x="120" y="260" width="60" height="120" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-274" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-5" target="6FWRLcRsoSDiZ6vq709X-272" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="810" y="260" /> <mxPoint x="810" y="260" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-275" value="succeeded" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-274" vertex="1" connectable="0"> <mxGeometry x="-0.0667" y="-3" relative="1" as="geometry"> <mxPoint y="-13" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-278" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-17" target="6FWRLcRsoSDiZ6vq709X-273" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="740" y="414" /> <mxPoint x="740" y="414" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-279" value="failed" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-278" vertex="1" connectable="0"> <mxGeometry x="0.2222" y="-3" relative="1" as="geometry"> <mxPoint x="8" y="-13" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-18" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=1;entryDx=0;entryDy=0;" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-2" target="6FWRLcRsoSDiZ6vq709X-2" edge="1"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-276" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-272" target="6FWRLcRsoSDiZ6vq709X-13" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="780" y="220" /> <mxPoint x="780" y="220" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-277" value="pa" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-276" vertex="1" connectable="0"> <mxGeometry x="-0.1556" y="-2" relative="1" as="geometry"> <mxPoint x="13" y="-8" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-272" value="&lt;b&gt;MRM_SUCCEEDED&lt;/b&gt;" style="rounded=0;whiteSpace=wrap;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" vertex="1"> <mxGeometry x="850" y="200" width="130" height="80" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-273" value="&lt;b&gt;MRM_FAILED&lt;/b&gt;" style="rounded=0;whiteSpace=wrap;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" vertex="1"> <mxGeometry x="850" y="370" width="130" height="80" as="geometry" /> </mxCell> </root> </mxGraphModel> </diagram> </mxfile> " > - - + + + + -
-
+
+
- not emergency + MRM_OPERATING
- not emergency + MRM_OPERATING - - - - - - + + -
+
- emergency -
-
-
- - emergency - - - - - - -
-
-
- NORMAL + pull over available (pa)
- NORMAL + pull over available (pa) - - + + -
+
- succeeded + pull over unavailable +
+ comfortable_stop unavailable +
+ emergency_stop available (ea)
- succeeded + pull over unavailable... - - + + -
+
- failed + pull over unavailable +
+ comfortable_stop available (ca)
- failed + pull over unavailable... - - - + + + +
-
- MRM_OPERATING +
+ pull_over
- MRM_OPERATING + pull_over - - + + -
+
- pull over unavailable -
- comfortable_stop available (ca) + ea || failed
- pull over unavailable... + ea || failed - - -
+
- pull over available (pa) + ea || failed
- pull over available (pa) + ea || failed + + + + + + +
+
+
+ emergency_stop +
+
+
+ + emergency_stop - - + + -
+
- pull over unavailable -
- comfortable_stop unavailable -
- emergency_stop available (ea) + ca
- pull over unavailable... + ca - - - - - - +
- pull_over + comfortable_stop
- pull_over + comfortable_stop - - + + -
+
- ea + not emergency
- ea + not emergency + + + + + -
+
- ea + emergency
- ea + emergency - + -
+
- emergency_stop + NORMAL
- emergency_stop + NORMAL - - + + -
+
- ca + succeeded
- ca + succeeded - + + -
-
-
- comfortable_stop +
+
+
+ failed
- comfortable_stop + failed - - + + -
+
- pa + pa @@ -334,13 +332,13 @@ MRM_SUCCEEDED - +
@@ -349,7 +347,7 @@
- MRM_FAILED + MRM_FAILED diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index 8a6ec81cbafff..1dd0f6b081337 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -47,10 +47,10 @@ struct Param { int update_rate; double timeout_operation_mode_availability; + double timeout_call_mrm_behavior; + double timeout_cancel_mrm_behavior; bool use_emergency_holding; double timeout_emergency_recovery; - double timeout_takeover_request; - bool use_takeover_request; bool use_parking_after_stopped; bool use_pull_over; bool use_comfortable_stop; @@ -63,6 +63,9 @@ class MrmHandler : public rclcpp::Node MrmHandler(); private: + // type + enum RequestType { CALL, CANCEL }; + // Subscribers rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr @@ -106,9 +109,8 @@ class MrmHandler : public rclcpp::Node pub_hazard_cmd_; rclcpp::Publisher::SharedPtr pub_gear_cmd_; - autoware_auto_vehicle_msgs::msg::HazardLightsCommand createHazardCmdMsg(); - autoware_auto_vehicle_msgs::msg::GearCommand createGearCmdMsg(); - void publishControlCommands(); + void publishHazardCmd(); + void publishGearCmd(); rclcpp::Publisher::SharedPtr pub_mrm_state_; @@ -123,10 +125,9 @@ class MrmHandler : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr client_mrm_emergency_stop_group_; rclcpp::Client::SharedPtr client_mrm_emergency_stop_; - void callMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const; - void cancelMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const; + bool requestMrmBehavior( + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, + RequestType request_type) const; void logMrmCallingResult( const tier4_system_msgs::srv::OperateMrm::Response & result, const std::string & behavior, bool is_call) const; @@ -143,14 +144,15 @@ class MrmHandler : public rclcpp::Node // Heartbeat rclcpp::Time stamp_operation_mode_availability_; std::optional stamp_autonomous_become_unavailable_ = std::nullopt; + bool is_operation_mode_availability_timeout; + void checkOperationModeAvailabilityTimeout(); // Algorithm - rclcpp::Time takeover_requested_time_; - bool is_takeover_request_ = false; bool is_emergency_holding_ = false; void transitionTo(const int new_state); void updateMrmState(); void operateMrm(); + void handleFailedRequest(); autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); bool isStopped(); bool isEmergency() const; diff --git a/system/mrm_handler/schema/mrm_handler.schema.json b/system/mrm_handler/schema/mrm_handler.schema.json index 86b18449cb734..9a50c6a326f01 100644 --- a/system/mrm_handler/schema/mrm_handler.schema.json +++ b/system/mrm_handler/schema/mrm_handler.schema.json @@ -16,6 +16,16 @@ "description": "If the input `operation_mode_availability` topic cannot be received for more than `timeout_operation_mode_availability`, vehicle will make an emergency stop.", "default": 0.5 }, + "timeout_call_mrm_behavior": { + "type": "number", + "description": "If a service is requested to the mrm operator and the response is not returned by `timeout_call_mrm_behavior`, the timeout occurs.", + "default": 0.01 + }, + "timeout_cancel_mrm_behavior": { + "type": "number", + "description": "If a service is requested to the mrm operator and the response is not returned by `timeout_cancel_mrm_behavior`, the timeout occurs.", + "default": 0.01 + }, "use_emergency_holding": { "type": "boolean", "description": "If this parameter is true, the handler does not recover to the NORMAL state.", @@ -26,16 +36,6 @@ "description": "If the duration of the EMERGENCY state is longer than `timeout_emergency_recovery`, it does not recover to the NORMAL state.", "default": 5.0 }, - "timeout_takeover_request": { - "type": "number", - "description": "Transition to MRR_OPERATING if the time from the last takeover request exceeds `timeout_takeover_request`.", - "default": 10.0 - }, - "use_takeover_request": { - "type": "boolean", - "description": "If this parameter is true, the handler will record the time and make take over request to the driver when emergency state occurs.", - "default": "false" - }, "use_parking_after_stopped": { "type": "boolean", "description": "If this parameter is true, it will publish PARKING shift command.", @@ -66,10 +66,10 @@ "required": [ "update_rate", "timeout_operation_mode_availability", + "timeout_call_mrm_behavior", + "timeout_cancel_mrm_behavior", "use_emergency_holding", "timeout_emergency_recovery", - "timeout_takeover_request", - "use_takeover_request", "use_parking_after_stopped", "use_pull_over", "use_comfortable_stop", diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index 976f5b3164abd..e8e692f755e2d 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -24,10 +24,11 @@ MrmHandler::MrmHandler() : Node("mrm_handler") param_.update_rate = declare_parameter("update_rate", 10); param_.timeout_operation_mode_availability = declare_parameter("timeout_operation_mode_availability", 0.5); + param_.timeout_call_mrm_behavior = declare_parameter("timeout_call_mrm_behavior", 0.01); + param_.timeout_cancel_mrm_behavior = + declare_parameter("timeout_cancel_mrm_behavior", 0.01); param_.use_emergency_holding = declare_parameter("use_emergency_holding", false); param_.timeout_emergency_recovery = declare_parameter("timeout_emergency_recovery", 5.0); - param_.timeout_takeover_request = declare_parameter("timeout_takeover_request", 10.0); - param_.use_takeover_request = declare_parameter("use_takeover_request", false); param_.use_parking_after_stopped = declare_parameter("use_parking_after_stopped", false); param_.use_pull_over = declare_parameter("use_pull_over", false); param_.use_comfortable_stop = declare_parameter("use_comfortable_stop", false); @@ -93,6 +94,7 @@ MrmHandler::MrmHandler() : Node("mrm_handler") mrm_state_.stamp = this->now(); mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL; mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE; + is_operation_mode_availability_timeout = false; // Timer const auto update_period_ns = rclcpp::Rate(param_.update_rate).period(); @@ -163,48 +165,39 @@ void MrmHandler::onOperationModeState( operation_mode_state_ = msg; } -autoware_auto_vehicle_msgs::msg::HazardLightsCommand MrmHandler::createHazardCmdMsg() +void MrmHandler::publishHazardCmd() { using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; HazardLightsCommand msg; - // Check emergency - const bool is_emergency = isEmergency(); - + msg.stamp = this->now(); if (is_emergency_holding_) { // turn hazard on during emergency holding msg.command = HazardLightsCommand::ENABLE; - } else if (is_emergency && param_.turning_hazard_on.emergency) { + } else if (isEmergency() && param_.turning_hazard_on.emergency) { // turn hazard on if vehicle is in emergency state and // turning hazard on if emergency flag is true msg.command = HazardLightsCommand::ENABLE; } else { msg.command = HazardLightsCommand::NO_COMMAND; } - return msg; + + pub_hazard_cmd_->publish(msg); } -void MrmHandler::publishControlCommands() +void MrmHandler::publishGearCmd() { using autoware_auto_vehicle_msgs::msg::GearCommand; + GearCommand msg; - // Create timestamp - const auto stamp = this->now(); - - // Publish hazard command - pub_hazard_cmd_->publish(createHazardCmdMsg()); - - // Publish gear - { - GearCommand msg; - msg.stamp = stamp; - if (param_.use_parking_after_stopped && isStopped()) { - msg.command = GearCommand::PARK; - } else { - msg.command = GearCommand::DRIVE; - } - pub_gear_cmd_->publish(msg); + msg.stamp = this->now(); + if (param_.use_parking_after_stopped && isStopped()) { + msg.command = GearCommand::PARK; + } else { + msg.command = GearCommand::DRIVE; } + + pub_gear_cmd_->publish(msg); } void MrmHandler::publishMrmState() @@ -220,18 +213,27 @@ void MrmHandler::operateMrm() if (mrm_state_.state == MrmState::NORMAL) { // Cancel MRM behavior when returning to NORMAL state const auto current_mrm_behavior = MrmState::NONE; - if (current_mrm_behavior != mrm_state_.behavior) { - cancelMrmBehavior(mrm_state_.behavior); + if (current_mrm_behavior == mrm_state_.behavior) { + return; + } + if (requestMrmBehavior(mrm_state_.behavior, RequestType::CANCEL)) { mrm_state_.behavior = current_mrm_behavior; + } else { + handleFailedRequest(); } return; } if (mrm_state_.state == MrmState::MRM_OPERATING) { const auto current_mrm_behavior = getCurrentMrmBehavior(); - if (current_mrm_behavior != mrm_state_.behavior) { - cancelMrmBehavior(mrm_state_.behavior); - callMrmBehavior(current_mrm_behavior); + if (current_mrm_behavior == mrm_state_.behavior) { + return; + } + if (!requestMrmBehavior(mrm_state_.behavior, RequestType::CANCEL)) { + handleFailedRequest(); + } else if (requestMrmBehavior(current_mrm_behavior, RequestType::CALL)) { mrm_state_.behavior = current_mrm_behavior; + } else { + handleFailedRequest(); } return; } @@ -247,88 +249,94 @@ void MrmHandler::operateMrm() RCLCPP_WARN(this->get_logger(), "invalid MRM state: %d", mrm_state_.state); } -void MrmHandler::callMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const +void MrmHandler::handleFailedRequest() { using autoware_adapi_v1_msgs::msg::MrmState; - auto request = std::make_shared(); - request->operate = true; - - if (mrm_behavior == MrmState::NONE) { - RCLCPP_WARN(this->get_logger(), "MRM behavior is None. Do nothing."); - return; - } - if (mrm_behavior == MrmState::PULL_OVER) { - auto result = client_mrm_pull_over_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Pull over is operated"); - } else { - RCLCPP_ERROR(this->get_logger(), "Pull over failed to operate"); - } - return; - } - if (mrm_behavior == MrmState::COMFORTABLE_STOP) { - auto result = client_mrm_comfortable_stop_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Comfortable stop is operated"); - } else { - RCLCPP_ERROR(this->get_logger(), "Comfortable stop failed to operate"); - } - return; - } - if (mrm_behavior == MrmState::EMERGENCY_STOP) { - auto result = client_mrm_emergency_stop_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Emergency stop is operated"); - } else { - RCLCPP_ERROR(this->get_logger(), "Emergency stop failed to operate"); - } - return; + if (requestMrmBehavior(MrmState::EMERGENCY_STOP, CALL)) { + if (mrm_state_.state != MrmState::MRM_OPERATING) transitionTo(MrmState::MRM_OPERATING); + } else { + transitionTo(MrmState::MRM_FAILED); } - RCLCPP_WARN(this->get_logger(), "invalid MRM behavior: %d", mrm_behavior); + mrm_state_.behavior = MrmState::EMERGENCY_STOP; } -void MrmHandler::cancelMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const +bool MrmHandler::requestMrmBehavior( + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, + RequestType request_type) const { using autoware_adapi_v1_msgs::msg::MrmState; auto request = std::make_shared(); - request->operate = false; - - if (mrm_behavior == MrmState::NONE) { - // Do nothing - return; + if (request_type == RequestType::CALL) { + request->operate = true; + } else if (request_type == RequestType::CANCEL) { + request->operate = false; + } else { + RCLCPP_ERROR(this->get_logger(), "invalid request type: %d", request_type); + return false; } - if (mrm_behavior == MrmState::PULL_OVER) { - auto result = client_mrm_pull_over_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Pull over is canceled"); - } else { - RCLCPP_ERROR(this->get_logger(), "Pull over failed to cancel"); + const auto duration = std::chrono::duration>( + request->operate ? param_.timeout_call_mrm_behavior : param_.timeout_cancel_mrm_behavior); + std::shared_future> future; + + const auto behavior2string = [](const int behavior) { + if (behavior == MrmState::NONE) { + return "NONE"; } - return; - } - if (mrm_behavior == MrmState::COMFORTABLE_STOP) { - auto result = client_mrm_comfortable_stop_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Comfortable stop is canceled"); - } else { - RCLCPP_ERROR(this->get_logger(), "Comfortable stop failed to cancel"); + if (behavior == MrmState::PULL_OVER) { + return "PULL_OVER"; } - return; + if (behavior == MrmState::COMFORTABLE_STOP) { + return "COMFORTABLE_STOP"; + } + if (behavior == MrmState::EMERGENCY_STOP) { + return "EMERGENCY_STOP"; + } + const auto msg = "invalid behavior: " + std::to_string(behavior); + throw std::runtime_error(msg); + }; + + switch (mrm_behavior) { + case MrmState::NONE: + RCLCPP_WARN(this->get_logger(), "MRM behavior is None. Do nothing."); + return true; + case MrmState::PULL_OVER: { + future = client_mrm_pull_over_->async_send_request(request).future.share(); + break; + } + case MrmState::COMFORTABLE_STOP: { + future = client_mrm_comfortable_stop_->async_send_request(request).future.share(); + break; + } + case MrmState::EMERGENCY_STOP: { + future = client_mrm_emergency_stop_->async_send_request(request).future.share(); + break; + } + default: + RCLCPP_ERROR(this->get_logger(), "invalid behavior: %d", mrm_behavior); + return false; } - if (mrm_behavior == MrmState::EMERGENCY_STOP) { - auto result = client_mrm_emergency_stop_->async_send_request(request).get(); + + if (future.wait_for(duration) == std::future_status::ready) { + const auto result = future.get(); if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Emergency stop is canceled"); + RCLCPP_WARN( + this->get_logger(), request->operate ? "%s is operated." : "%s is canceled.", + behavior2string(mrm_behavior)); + return true; } else { - RCLCPP_ERROR(this->get_logger(), "Emergency stop failed to cancel"); + RCLCPP_ERROR( + this->get_logger(), request->operate ? "%s failed to operate." : "%s failed to cancel.", + behavior2string(mrm_behavior)); + return false; } - return; + } else { + RCLCPP_ERROR( + this->get_logger(), request->operate ? "%s call timed out." : "%s cancel timed out.", + behavior2string(mrm_behavior)); + return false; } - RCLCPP_WARN(this->get_logger(), "invalid MRM behavior: %d", mrm_behavior); } bool MrmHandler::isDataReady() @@ -369,30 +377,39 @@ bool MrmHandler::isDataReady() return true; } -void MrmHandler::onTimer() +void MrmHandler::checkOperationModeAvailabilityTimeout() { - if (!isDataReady()) { - return; - } - const bool is_operation_mode_availability_timeout = + if ( (this->now() - stamp_operation_mode_availability_).seconds() > - param_.timeout_operation_mode_availability; - if (is_operation_mode_availability_timeout) { + param_.timeout_operation_mode_availability) { + is_operation_mode_availability_timeout = true; RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), - "heartbeat operation_mode_availability is timeout"); - mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING; - publishControlCommands(); + "operation_mode_availability is timeout"); + } else { + is_operation_mode_availability_timeout = false; + } +} + +void MrmHandler::onTimer() +{ + if (!isDataReady()) { return; } + // Check whether operation_mode_availability is timeout + checkOperationModeAvailabilityTimeout(); + // Update Emergency State updateMrmState(); - // Publish control commands - publishControlCommands(); + // Operate MRM operateMrm(); + + // Publish publishMrmState(); + publishHazardCmd(); + publishGearCmd(); } void MrmHandler::transitionTo(const int new_state) @@ -434,41 +451,23 @@ void MrmHandler::updateMrmState() // Get mode const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS; - const bool is_takeover_done = control_mode_->mode == ControlModeReport::MANUAL; // State Machine if (mrm_state_.state == MrmState::NORMAL) { // NORMAL if (is_auto_mode && is_emergency) { - if (param_.use_takeover_request) { - takeover_requested_time_ = this->get_clock()->now(); - is_takeover_request_ = true; - return; - } else { - transitionTo(MrmState::MRM_OPERATING); - return; - } + transitionTo(MrmState::MRM_OPERATING); + return; } } else { // Emergency - // Send recovery events if "not emergency" or "takeover done" + // Send recovery events if "not emergency" if (!is_emergency) { transitionTo(MrmState::NORMAL); return; } - // TODO(TetsuKawa): Check if human can safely override, for example using DSM - if (is_takeover_done) { - transitionTo(MrmState::NORMAL); - return; - } - if (is_takeover_request_) { - const auto time_from_takeover_request = this->get_clock()->now() - takeover_requested_time_; - if (time_from_takeover_request.seconds() > param_.timeout_takeover_request) { - transitionTo(MrmState::MRM_OPERATING); - return; - } - } else if (mrm_state_.state == MrmState::MRM_OPERATING) { + if (mrm_state_.state == MrmState::MRM_OPERATING) { // TODO(TetsuKawa): Check MRC is accomplished if (mrm_state_.behavior == MrmState::PULL_OVER) { if (isStopped() && isArrivedAtGoal()) { @@ -502,6 +501,9 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB // State machine if (mrm_state_.behavior == MrmState::NONE) { + if (is_operation_mode_availability_timeout) { + return MrmState::EMERGENCY_STOP; + } if (operation_mode_availability_->pull_over) { if (param_.use_pull_over) { return MrmState::PULL_OVER; @@ -518,6 +520,9 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB return MrmState::EMERGENCY_STOP; } if (mrm_state_.behavior == MrmState::PULL_OVER) { + if (is_operation_mode_availability_timeout) { + return MrmState::EMERGENCY_STOP; + } if (operation_mode_availability_->pull_over) { if (param_.use_pull_over) { return MrmState::PULL_OVER; @@ -534,6 +539,9 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB return MrmState::EMERGENCY_STOP; } if (mrm_state_.behavior == MrmState::COMFORTABLE_STOP) { + if (is_operation_mode_availability_timeout) { + return MrmState::EMERGENCY_STOP; + } if (isStopped() && operation_mode_availability_->pull_over) { if (param_.use_pull_over) { return MrmState::PULL_OVER; @@ -550,6 +558,9 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB return MrmState::EMERGENCY_STOP; } if (mrm_state_.behavior == MrmState::EMERGENCY_STOP) { + if (is_operation_mode_availability_timeout) { + return MrmState::EMERGENCY_STOP; + } if (isStopped() && operation_mode_availability_->pull_over) { if (param_.use_pull_over) { return MrmState::PULL_OVER; @@ -576,7 +587,8 @@ bool MrmHandler::isStopped() bool MrmHandler::isEmergency() const { - return !operation_mode_availability_->autonomous || is_emergency_holding_; + return !operation_mode_availability_->autonomous || is_emergency_holding_ || + is_operation_mode_availability_timeout; } bool MrmHandler::isArrivedAtGoal() diff --git a/vehicle/raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml b/vehicle/raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml index 1001696d96679..bce50a0b581c7 100644 --- a/vehicle/raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml +++ b/vehicle/raw_vehicle_cmd_converter/config/raw_vehicle_cmd_converter.param.yaml @@ -1,5 +1,8 @@ /**: ros__parameters: + csv_path_accel_map: $(find-pkg-share raw_vehicle_cmd_converter)/data/default/accel_map.csv + csv_path_brake_map: $(find-pkg-share raw_vehicle_cmd_converter)/data/default/brake_map.csv + csv_path_steer_map: $(find-pkg-share raw_vehicle_cmd_converter)/data/default/steer_map.csv convert_accel_cmd: true convert_brake_cmd: true convert_steer_cmd: true diff --git a/vehicle/raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml b/vehicle/raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml index a74cdcc56132d..0a9962a7c2a30 100644 --- a/vehicle/raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml +++ b/vehicle/raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml @@ -1,9 +1,5 @@ - - - - @@ -13,9 +9,6 @@ - - - diff --git a/vehicle/raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json b/vehicle/raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json index cc292c5a8c40f..4b562f401e09b 100644 --- a/vehicle/raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json +++ b/vehicle/raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json @@ -6,6 +6,21 @@ "raw_vehicle_cmd_converter": { "type": "object", "properties": { + "csv_path_accel_map": { + "type": "string", + "description": "path for acceleration map csv file", + "default": "$(find-pkg-share raw_vehicle_cmd_converter)/data/default/accel_map.csv" + }, + "csv_path_brake_map": { + "type": "string", + "description": "path for brake map csv file", + "default": "$(find-pkg-share raw_vehicle_cmd_converter)/data/default/brake_map.csv" + }, + "csv_path_steer_map": { + "type": "string", + "description": "path for steer map csv file", + "default": "$(find-pkg-share raw_vehicle_cmd_converter)/data/default/steer_map.csv" + }, "convert_accel_cmd": { "type": "boolean", "description": "use accel or not", @@ -142,6 +157,9 @@ } }, "required": [ + "csv_path_accel_map", + "csv_path_brake_map", + "csv_path_steer_map", "convert_accel_cmd", "convert_brake_cmd", "convert_steer_cmd",