diff --git a/.cspell-partial.json b/.cspell-partial.json index f45ae8961c56b..e231eddd712ea 100644 --- a/.cspell-partial.json +++ b/.cspell-partial.json @@ -5,5 +5,5 @@ "perception/bytetrack/lib/**" ], "ignoreRegExpList": [], - "words": [] + "words": ["dltype", "tvmgen", "quantizer", "imageio", "mimsave"] } diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 9de27f82ffcef..e298dccefd827 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -88,13 +88,15 @@ launch/tier4_system_launch/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp localization/ekf_localizer/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp localization/gyro_odometer/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -localization/landmark_based_localizer/ar_tag_based_localizer/** masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp -localization/landmark_based_localizer/landmark_tf_caster/** masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp +localization/landmark_based_localizer/ar_tag_based_localizer/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp +localization/landmark_based_localizer/landmark_parser/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp localization/localization_error_monitor/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/localization_util/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp localization/ndt_scan_matcher/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/pose2twist/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/pose_initializer/** isamu.takagi@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/stop_filter/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/tree_structured_parzen_estimator/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp localization/twist2accel/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_common/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp localization/yabloc/yabloc_image_processing/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp @@ -152,7 +154,7 @@ perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp perception/traffic_light_visualization/** yukihiro.saito@tier4.jp planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_crosswalk_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_crosswalk_module/** kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_intersection_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @@ -179,7 +181,7 @@ planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.j planning/obstacle_stop_planner/** berkay@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp planning/path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp -planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp +planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/planning_test_utils/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp planning/planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp @@ -221,7 +223,6 @@ system/emergency_handler/** makoto.kurihara@tier4.jp system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp system/system_diagnostic_graph/** isamu.takagi@tier4.jp -system/system_diagnostic_monitor/** isamu.takagi@tier4.jp system/system_error_monitor/** fumihito.ito@tier4.jp system/system_monitor/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp system/topic_state_monitor/** ryohsuke.mitsudome@tier4.jp diff --git a/.github/dependabot.yaml b/.github/dependabot.yaml index 3f3bf243f639d..0264c035357bd 100644 --- a/.github/dependabot.yaml +++ b/.github/dependabot.yaml @@ -6,5 +6,5 @@ updates: interval: daily open-pull-requests-limit: 1 labels: - - bot - - github-actions + - tag:bot + - type:github-actions diff --git a/.github/labeler.yaml b/.github/labeler.yaml index 205a24fc7e8f6..c3efa2a1a2b15 100644 --- a/.github/labeler.yaml +++ b/.github/labeler.yaml @@ -1,4 +1,4 @@ -ci: +"type:ci": - .github/**/* - "*.json" - "*.yaml" @@ -6,36 +6,36 @@ ci: - .clang-format - .gitignore - .prettierignore -documentation: +"type:documentation": - docs/**/* - "**/*.md" - "**/*.rst" - "**/*.jpg" - "**/*.png" - "**/*.svg" -common: +"component:common": - common/**/* -control: +"component:control": - control/**/* -evaluator: +"component:evaluator": - evaluator/**/* -launch: +"component:launch": - launch/**/* -localization: +"component:localization": - localization/**/* -map: +"component:map": - map/**/* -perception: +"component:perception": - perception/**/* -planning: +"component:planning": - planning/**/* -sensing: +"component:sensing": - sensing/**/* -simulator: +"component:simulator": - simulator/**/* -system: +"component:system": - system/**/* -tools: +"component:tools": - tools/**/* -vehicle: +"component:vehicle": - vehicle/**/* diff --git a/.github/stale.yml b/.github/stale.yml index 84928d1b815ee..bc99e4383cafd 100644 --- a/.github/stale.yml +++ b/.github/stale.yml @@ -4,7 +4,7 @@ daysUntilClose: false # Label to use when marking as stale -staleLabel: stale +staleLabel: status:stale # Comment to post when marking as stale markComment: > diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index f9cb3065f12a7..4d316b9fb2481 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -12,6 +12,8 @@ - source: .github/PULL_REQUEST_TEMPLATE/standard-change.md - source: .github/dependabot.yaml - source: .github/stale.yml + pre-commands: | + sd "staleLabel: stale" "staleLabel: status:stale" {source} - source: .github/workflows/cancel-previous-workflows.yaml - source: .github/workflows/github-release.yaml - source: .github/workflows/pre-commit.yaml @@ -84,4 +86,8 @@ sd "/edit/main/docs/" "/edit/main/" {source} sd "docs_dir: .*" "docs_dir: ." {source} sd "assets/(\w+)" "docs/assets/\$1" {source} + sd -- \ + " - macros" \ + " - macros: + module_name: mkdocs_macros" {source} - source: docs/assets/js/mathjax.js diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index e32dccda70b25..e95f4d46a147b 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -11,7 +11,7 @@ jobs: prevent-no-label-execution: uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 with: - label: run-build-and-test-differential + label: tag:run-build-and-test-differential build-and-test-differential: needs: prevent-no-label-execution diff --git a/.github/workflows/deploy-docs.yaml b/.github/workflows/deploy-docs.yaml index 5f2d09d414806..b48d70dbacb0c 100644 --- a/.github/workflows/deploy-docs.yaml +++ b/.github/workflows/deploy-docs.yaml @@ -22,7 +22,7 @@ jobs: prevent-no-label-execution: uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 with: - label: deploy-docs + label: tag:deploy-docs deploy-docs: needs: prevent-no-label-execution diff --git a/.github/workflows/openai-pr-reviewer.yaml b/.github/workflows/openai-pr-reviewer.yaml index 0ba732b580204..4ecbd93198276 100644 --- a/.github/workflows/openai-pr-reviewer.yaml +++ b/.github/workflows/openai-pr-reviewer.yaml @@ -21,7 +21,7 @@ jobs: prevent-no-label-execution: uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 with: - label: openai-pr-reviewer + label: tag:openai-pr-reviewer review: needs: prevent-no-label-execution if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} diff --git a/.github/workflows/sync-files.yaml b/.github/workflows/sync-files.yaml index b9dc5907a50c4..5025e6c8bd7a7 100644 --- a/.github/workflows/sync-files.yaml +++ b/.github/workflows/sync-files.yaml @@ -28,6 +28,6 @@ jobs: with: token: ${{ steps.generate-token.outputs.token }} pr-labels: | - bot - sync-files + tag:bot + tag:sync-files auto-merge-method: squash diff --git a/.github/workflows/update-codeowners-from-packages.yaml b/.github/workflows/update-codeowners-from-packages.yaml index 055d2b02a799a..7898dfe09177a 100644 --- a/.github/workflows/update-codeowners-from-packages.yaml +++ b/.github/workflows/update-codeowners-from-packages.yaml @@ -28,6 +28,6 @@ jobs: with: token: ${{ steps.generate-token.outputs.token }} pr-labels: | - bot - update-codeowners-from-packages + tag:bot + tag:update-codeowners-from-packages auto-merge-method: squash diff --git a/common/interpolation/package.xml b/common/interpolation/package.xml index e5d2c7af8d06e..6be04482da3ee 100644 --- a/common/interpolation/package.xml +++ b/common/interpolation/package.xml @@ -6,7 +6,6 @@ The spline interpolation package Fumiya Watanabe Takayuki Murooka - Yutaka Shimizu Apache License 2.0 ament_cmake_auto diff --git a/common/kalman_filter/include/kalman_filter/kalman_filter.hpp b/common/kalman_filter/include/kalman_filter/kalman_filter.hpp index 79a47bde3a1b2..b500cffb92279 100644 --- a/common/kalman_filter/include/kalman_filter/kalman_filter.hpp +++ b/common/kalman_filter/include/kalman_filter/kalman_filter.hpp @@ -109,20 +109,20 @@ class KalmanFilter * @brief get current kalman filter state * @param x kalman filter state */ - void getX(Eigen::MatrixXd & x); + void getX(Eigen::MatrixXd & x) const; /** * @brief get current kalman filter covariance * @param P kalman filter covariance */ - void getP(Eigen::MatrixXd & P); + void getP(Eigen::MatrixXd & P) const; /** * @brief get component of current kalman filter state * @param i index of kalman filter state * @return value of i's component of the kalman filter state x[i] */ - double getXelement(unsigned int i); + double getXelement(unsigned int i) const; /** * @brief calculate kalman filter state and covariance by prediction model with A, B, Q matrix. diff --git a/common/kalman_filter/src/kalman_filter.cpp b/common/kalman_filter/src/kalman_filter.cpp index 937819ffb0275..450d40936db2e 100644 --- a/common/kalman_filter/src/kalman_filter.cpp +++ b/common/kalman_filter/src/kalman_filter.cpp @@ -77,15 +77,15 @@ void KalmanFilter::setR(const Eigen::MatrixXd & R) { R_ = R; } -void KalmanFilter::getX(Eigen::MatrixXd & x) +void KalmanFilter::getX(Eigen::MatrixXd & x) const { x = x_; } -void KalmanFilter::getP(Eigen::MatrixXd & P) +void KalmanFilter::getP(Eigen::MatrixXd & P) const { P = P_; } -double KalmanFilter::getXelement(unsigned int i) +double KalmanFilter::getXelement(unsigned int i) const { return x_(i); } diff --git a/common/motion_utils/package.xml b/common/motion_utils/package.xml index 334b023f7f29a..5f630572c061c 100644 --- a/common/motion_utils/package.xml +++ b/common/motion_utils/package.xml @@ -4,7 +4,6 @@ motion_utils 0.1.0 The motion_utils package - Yutaka Shimizu Satoshi Ota Takayuki Murooka @@ -16,7 +15,6 @@ Mamoru Sobue Apache License 2.0 - Yutaka Shimizu Takayuki Murooka Satoshi Ota diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index 701907929ecc0..e37cd0cc33975 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -6,7 +6,6 @@ The tier4_autoware_utils package Takamasa Horibe Takayuki Murooka - Yutaka Shimizu Mamoru Sobue Apache License 2.0 diff --git a/common/tier4_camera_view_rviz_plugin/CMakeLists.txt b/common/tier4_camera_view_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..8ae8efef1e4b9 --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_camera_view_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wno-unused-parameter) +endif() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/third_person_view_controller.cpp + src/third_person_view_tool.cpp + src/bird_eye_view_tool.cpp + src/bird_eye_view_controller.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package(INSTALL_TO_SHARE icons) diff --git a/common/tier4_camera_view_rviz_plugin/README.md b/common/tier4_camera_view_rviz_plugin/README.md new file mode 100644 index 0000000000000..99480d5a92e1c --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/README.md @@ -0,0 +1,9 @@ +# tier4_camera_view_rviz_plugin + +## ThirdPersonView Tool + +Add the `tier4_camera_view_rviz_plugin/ThirdPersonViewTool` tool to the RViz. Push the button, the camera will focus on the vehicle and set the target frame to `base_link`. Short cut key 'o'. + +## BirdEyeView Tool + +Add the `tier4_camera_view_rviz_plugin/BirdEyeViewTool` tool to the RViz. Push the button, the camera will turn to the BEV view, the target frame is consistent with the latest frame. Short cut key 'r'. diff --git a/common/tier4_camera_view_rviz_plugin/icons/classes/BirdEyeViewTool.png b/common/tier4_camera_view_rviz_plugin/icons/classes/BirdEyeViewTool.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/tier4_camera_view_rviz_plugin/icons/classes/BirdEyeViewTool.png differ diff --git a/common/tier4_camera_view_rviz_plugin/icons/classes/ThirdPersonViewTool.png b/common/tier4_camera_view_rviz_plugin/icons/classes/ThirdPersonViewTool.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/tier4_camera_view_rviz_plugin/icons/classes/ThirdPersonViewTool.png differ diff --git a/common/tier4_camera_view_rviz_plugin/package.xml b/common/tier4_camera_view_rviz_plugin/package.xml new file mode 100644 index 0000000000000..0ede5e37b8287 --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/package.xml @@ -0,0 +1,31 @@ + + + + tier4_camera_view_rviz_plugin + 0.0.0 + The autoware camera view rviz plugin package + Yuxuan Liu + Makoto Yabuta + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_ad_api_specs + component_interface_utils + geometry_msgs + libqt5-core + libqt5-gui + libqt5-widgets + rclcpp + rviz_common + rviz_default_plugins + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/tier4_camera_view_rviz_plugin/plugins/plugin_description.xml b/common/tier4_camera_view_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..ab522b80de26a --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,24 @@ + + + + + Control the camera for bird-eye view. + + + + + Bird-eye-view Tool. This tool requires the corresponding BirdEyeViewController. + + + + + Control the camera for third-person view. + + + + + Third-person-view Tool. This tool requires the corresponding ThirdPersonViewController. + + + + diff --git a/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp new file mode 100644 index 0000000000000..bd5c3349d3c35 --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.cpp @@ -0,0 +1,242 @@ +// Copyright 2023 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. +/* + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "bird_eye_view_controller.hpp" + +#include "rviz_common/display_context.hpp" +#include "rviz_common/properties/bool_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/viewport_mouse_event.hpp" +#include "rviz_rendering/objects/shape.hpp" +#include "rviz_rendering/orthographic.hpp" + +#include +#include +#include +#include +#include +#include + +namespace tier4_camera_view_rviz_plugin +{ +static const Ogre::Quaternion ROBOT_TO_CAMERA_ROTATION = + Ogre::Quaternion(Ogre::Radian(-Ogre::Math::HALF_PI), Ogre::Vector3::UNIT_Y) * + Ogre::Quaternion(Ogre::Radian(-Ogre::Math::HALF_PI), Ogre::Vector3::UNIT_Z); + +static const float PITCH_LIMIT_LOW = -Ogre::Math::HALF_PI + 0.001; +static const float PITCH_LIMIT_HIGH = Ogre::Math::HALF_PI - 0.001; + +BirdEyeViewController::BirdEyeViewController() : dragging_(false) +{ + scale_property_ = new rviz_common::properties::FloatProperty( + "Scale", 10, "How much to scale up the size of things in the scene.", this); + angle_property_ = new rviz_common::properties::FloatProperty( + "Angle", 0, "Angle around the Z axis to rotate.", this); + x_property_ = + new rviz_common::properties::FloatProperty("X", 0, "X component of camera position.", this); + y_property_ = + new rviz_common::properties::FloatProperty("Y", 0, "Y component of camera position.", this); +} + +BirdEyeViewController::~BirdEyeViewController() +{ +} + +void BirdEyeViewController::onInitialize() +{ + FramePositionTrackingViewController::onInitialize(); + + camera_->setProjectionType(Ogre::PT_ORTHOGRAPHIC); + auto camera_parent = getCameraParent(camera_); + camera_parent->setFixedYawAxis(false); + invert_z_->hide(); +} + +void BirdEyeViewController::reset() +{ + scale_property_->setFloat(10); + angle_property_->setFloat(0); + x_property_->setFloat(0); + y_property_->setFloat(0); +} + +void BirdEyeViewController::handleMouseEvent(rviz_common::ViewportMouseEvent & event) +{ + if (event.shift()) { + setStatus("Left-Click: Move X/Y."); + } else { + setStatus( + "Left-Click: Rotate. Middle-Click: Move X/Y. Right-Click:: Zoom. " + "Shift: More options."); + } + + bool moved = false; + + int32_t diff_x = 0; + int32_t diff_y = 0; + + if (event.type == QEvent::MouseButtonPress) { + dragging_ = true; + } else if (event.type == QEvent::MouseButtonRelease) { + dragging_ = false; + } else if (dragging_ && event.type == QEvent::MouseMove) { + diff_x = event.x - event.last_x; + diff_y = event.y - event.last_y; + moved = true; + } + + if (event.left() && !event.shift()) { + setCursor(Rotate2D); + angle_property_->add(diff_x * 0.005); + orientCamera(); + } else if (event.middle() || (event.shift() && event.left())) { + setCursor(MoveXY); + float scale = scale_property_->getFloat(); + move_camera(-diff_x / scale, diff_y / scale); + } else if (event.right()) { + setCursor(Zoom); + scale_property_->multiply(1.0 - diff_y * 0.01); + } else { + setCursor(event.shift() ? MoveXY : Rotate2D); + } + + if (event.wheel_delta != 0) { + int diff = event.wheel_delta; + scale_property_->multiply(1.0 - (-diff) * 0.001); + + moved = true; + } + + if (moved) { + context_->queueRender(); + emitConfigChanged(); + } +} + +void BirdEyeViewController::orientCamera() +{ + camera_->setOrientation( + Ogre::Quaternion(Ogre::Radian(angle_property_->getFloat()), Ogre::Vector3::UNIT_Z)); +} + +void BirdEyeViewController::mimic(rviz_common::ViewController * source_view) +{ + FramePositionTrackingViewController::mimic(source_view); + + if (BirdEyeViewController * source_ortho = qobject_cast(source_view)) { + scale_property_->setFloat(source_ortho->scale_property_->getFloat()); + angle_property_->setFloat(source_ortho->angle_property_->getFloat()); + x_property_->setFloat(source_ortho->x_property_->getFloat()); + y_property_->setFloat(source_ortho->y_property_->getFloat()); + } else { + auto source_camera_parent = getCameraParent(source_view->getCamera()); + setPosition(source_camera_parent->getPosition()); + } +} +void BirdEyeViewController::update(float dt, float ros_dt) +{ + FramePositionTrackingViewController::update(dt, ros_dt); + updateCamera(); +} + +void BirdEyeViewController::lookAt(const Ogre::Vector3 & point) +{ + setPosition(point - target_scene_node_->getPosition()); +} + +void BirdEyeViewController::onTargetFrameChanged( + const Ogre::Vector3 & old_reference_position, + const Ogre::Quaternion & /*old_reference_orientation*/) +{ + move_camera( + old_reference_position.x - reference_position_.x, + old_reference_position.y - reference_position_.y); +} + +void BirdEyeViewController::updateCamera() +{ + orientCamera(); + + float width = camera_->getViewport()->getActualWidth(); + float height = camera_->getViewport()->getActualHeight(); + + float scale = scale_property_->getFloat(); + Ogre::Matrix4 proj = rviz_rendering::buildScaledOrthoMatrix( + -width / scale / 2, width / scale / 2, -height / scale / 2, height / scale / 2, + camera_->getNearClipDistance(), camera_->getFarClipDistance()); + camera_->setCustomProjectionMatrix(true, proj); + + // For Z, we use half of the far-clip distance set in + // selection_context.cpp, so that the shader program which computes + // depth can see equal distances above and below the Z=0 plane. + auto camera_parent = getCameraParent(camera_); + camera_parent->setPosition(x_property_->getFloat(), y_property_->getFloat(), 500); +} + +Ogre::SceneNode * BirdEyeViewController::getCameraParent(Ogre::Camera * camera) +{ + auto camera_parent = camera->getParentSceneNode(); + + if (!camera_parent) { + throw std::runtime_error("camera's parent scene node pointer unexpectedly nullptr"); + } + return camera_parent; +} + +void BirdEyeViewController::setPosition(const Ogre::Vector3 & pos_rel_target) +{ + x_property_->setFloat(pos_rel_target.x); + y_property_->setFloat(pos_rel_target.y); +} + +void BirdEyeViewController::move_camera(float dx, float dy) +{ + float angle = angle_property_->getFloat(); + x_property_->add(dx * cos(angle) - dy * sin(angle)); + y_property_->add(dx * sin(angle) + dy * cos(angle)); +} + +} // namespace tier4_camera_view_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS( + tier4_camera_view_rviz_plugin::BirdEyeViewController, rviz_common::ViewController) diff --git a/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.hpp b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.hpp new file mode 100644 index 0000000000000..3c0091740bd59 --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_controller.hpp @@ -0,0 +1,112 @@ +// Copyright 2023 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. +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef BIRD_EYE_VIEW_CONTROLLER_HPP_ +#define BIRD_EYE_VIEW_CONTROLLER_HPP_ + +#include "rviz_common/frame_position_tracking_view_controller.hpp" + +#include +#include + +namespace rviz_common +{ +namespace properties +{ +class FloatProperty; +class Shape; +class VectorProperty; +} // namespace properties +} // namespace rviz_common + +namespace tier4_camera_view_rviz_plugin +{ +/** @brief A first-person camera, controlled by yaw, pitch, and position. */ +class BirdEyeViewController : public rviz_common::FramePositionTrackingViewController +{ + Q_OBJECT + +public: + BirdEyeViewController(); + + ~BirdEyeViewController() override; + + void onInitialize() override; + + void handleMouseEvent(rviz_common::ViewportMouseEvent & evt) override; + + void lookAt(const Ogre::Vector3 & point) override; + + void reset() override; + + /** @brief Configure the settings of this view controller to give, + * as much as possible, a similar view as that given by the + * @param source_view. + * + * @param source_view must return a valid @c Ogre::Camera* from getCamera(). */ + void mimic(rviz_common::ViewController * source_view) override; + + void update(float dt, float ros_dt) override; + +protected: + void onTargetFrameChanged( + const Ogre::Vector3 & old_reference_position, + const Ogre::Quaternion & old_reference_orientation) override; + + /** Set the camera orientation based on angle_. */ + void orientCamera(); + + void setPosition(const Ogre::Vector3 & pos_rel_target); + void move_camera(float x, float y); + void updateCamera(); + Ogre::SceneNode * getCameraParent(Ogre::Camera * camera); + + rviz_common::properties::FloatProperty * scale_property_; + rviz_common::properties::FloatProperty * angle_property_; + rviz_common::properties::FloatProperty * x_property_; + rviz_common::properties::FloatProperty * y_property_; + bool dragging_; +}; + +} // namespace tier4_camera_view_rviz_plugin + +#endif // BIRD_EYE_VIEW_CONTROLLER_HPP_ diff --git a/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_tool.cpp b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_tool.cpp new file mode 100644 index 0000000000000..0c50f1ae1a0f4 --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_tool.cpp @@ -0,0 +1,167 @@ +// Copyright 2023 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. +/* + * UOS-ROS packages - Robot Operating System code by the University of Osnabrück + * Copyright (C) 2014 University of Osnabrück + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted + * provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this list of conditions + * and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, this list of + * conditions and the following disclaimer in the documentation and/or other materials provided with + * the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR + * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND + * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY + * WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * route_set_tool.cpp + * + * Author: Henning Deeken {hdeeken@uos.de} + * + */ + +#include "bird_eye_view_tool.hpp" + +#include "rviz_common/display_context.hpp" +#include "rviz_common/view_manager.hpp" + +#include + +namespace tier4_camera_view_rviz_plugin +{ +BirdEyeViewTool::BirdEyeViewTool() +{ + shortcut_key_ = 'r'; +} + +BirdEyeViewTool::~BirdEyeViewTool() +{ +} + +void BirdEyeViewTool::onInitialize() +{ + setName("Bird Eye View"); + + step_length_property_ = new rviz_common::properties::FloatProperty( + "Step Length", 0.1, "The length by with the position is updated on each step.", + getPropertyContainer(), SLOT(setOffset()), this); + + boost_property_ = new rviz_common::properties::FloatProperty( + "Boost Property", 0.5, "Gives the boost factor which is applied if pressing shift.", + getPropertyContainer(), SLOT(setBoost()), this); + + fly_property_ = new rviz_common::properties::BoolProperty( + "Fly Mode", false, "In fly mode it is possible to move along the z axis as well.", + getPropertyContainer(), SLOT(setFlyMode()), this); + + left_hand_property_ = new rviz_common::properties::BoolProperty( + "Left Hand Mode", false, "In left hand mode one uses the arrows to move around", + getPropertyContainer(), SLOT(setLeftHandMode()), this); + + fallback_view_controller_property_ = new rviz_common::properties::EnumProperty( + "Fallback ViewController", QString("tier4_camera_view_rviz_plugin/BirdEyeView"), + "Determines to which view controller the control switches, if the tool is deactivated.", + getPropertyContainer(), SLOT(setFallbackViewController()), this); + + m_pos_offset = 0.1; + m_boost = 0.5; + m_fly_mode = false; + m_left_hand_mode = false; + + // temporarily disabled + // setFallbackToolProperty(); + setFallbackViewControllerProperty(); +} + +void BirdEyeViewTool::setFallbackViewControllerProperty() +{ + fallback_view_controller_property_->clearOptions(); + m_view_controller_classes.clear(); + + m_view_controller_classes = context_->getViewManager()->getDeclaredClassIdsFromFactory(); + + for (int i = 0; i < m_view_controller_classes.size(); i++) { + if (m_view_controller_classes[i] != QString("tier4_camera_view_rviz_plugin/BirdEyeView")) { + fallback_view_controller_property_->addOption(m_view_controller_classes[i], i); + m_view_controller.push_back(context_->getViewManager()->getViewAt(i)); + } + } + + fallback_view_controller_property_->show(); + setFallbackViewController(); +} + +// temporarily disabled +// void BirdEyeViewTool::setFallbackToolProperty() +// { +// fallback_tool_property_->clearOptions(); +// m_tools.clear(); + +// m_tool_classes = context_->getToolManager()->getToolClasses(); + +// for (int i = 0; i < m_tool_classes.size(); i++) { +// if (m_tool_classes[i] != getClassId()) { +// fallback_tool_property_->addOption(m_tool_classes[i], i); +// m_tools.push_back(context_->getToolManager()->getTool(i)); +// } +// } + +// fallback_tool_property_->show(); +// setFallbackTool(); +// } + +void BirdEyeViewTool::activate() +{ + context_->getViewManager()->setCurrentViewControllerType( + QString("tier4_camera_view_rviz_plugin/BirdEyeView")); + context_->getViewManager()->getCurrent()->reset(); + + // temporarily disabled + // setFallbackToolProperty(); + setFallbackViewControllerProperty(); +} + +void BirdEyeViewTool::deactivate() +{ +} + +int BirdEyeViewTool::processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel) +{ + return 0; +} + +int BirdEyeViewTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) +{ + if (event.panel->getViewController()) { + event.panel->getViewController()->handleMouseEvent(event); + setCursor(event.panel->getViewController()->getCursor()); + } + return 0; +} + +} // namespace tier4_camera_view_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS(tier4_camera_view_rviz_plugin::BirdEyeViewTool, rviz_common::Tool) diff --git a/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_tool.hpp b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_tool.hpp new file mode 100644 index 0000000000000..212cdd49c13bd --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/src/bird_eye_view_tool.hpp @@ -0,0 +1,144 @@ +// Copyright 2023 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. +/* + * UOS-ROS packages - Robot Operating System code by the University of Osnabrück + * Copyright (C) 2014 University of Osnabrück + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted + * provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this list of conditions + * and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, this list of + * conditions and the following disclaimer in the documentation and/or other materials provided with + * the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR + * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND + * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * route_set_tool.h + * + * Author: Henning Deeken + * + */ + +#ifndef BIRD_EYE_VIEW_TOOL_HPP_ +#define BIRD_EYE_VIEW_TOOL_HPP_ + +#include "bird_eye_view_controller.hpp" +#include "rviz_common/properties/bool_property.hpp" +#include "rviz_common/properties/enum_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/render_panel.hpp" +#include "rviz_common/tool.hpp" +#include "rviz_common/viewport_mouse_event.hpp" + +#include +#include +#include + +#include +#include + +/** + * + *@class BirdEyeViewTool + * + *@brief Implements a rviz tool that allows to navigate in a ego-shooter mode. + */ + +namespace tier4_camera_view_rviz_plugin +{ + +class FPSMotionConfigWidget; +class BirdEyeViewTool : public rviz_common::Tool +{ + Q_OBJECT + +public: + BirdEyeViewTool(); + ~BirdEyeViewTool(); + virtual void onInitialize(); + virtual void activate(); + virtual void deactivate(); + + virtual int processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel); + virtual int processMouseEvent(rviz_common::ViewportMouseEvent & event); + +private Q_SLOTS: + void setOffset() { m_pos_offset = static_cast(step_length_property_->getFloat()); } + void setBoost() + { + if (boost_property_->getFloat() < 0.0) { + m_boost = 0.0; + } else if (boost_property_->getFloat() > 1.0) { + m_boost = 1.0; + } else { + m_boost = static_cast(boost_property_->getFloat()); + } + } + + void setFlyMode() { m_fly_mode = fly_property_->getBool(); } + void setLeftHandMode() { m_left_hand_mode = left_hand_property_->getBool(); } + // temporarily disabled + // void setFallbackTool() { m_fallback_tool = m_tools.at(fallback_tool_property_->getOptionInt()); + // } + void setFallbackViewController() + { + m_fallback_view_controller = + m_view_controller_classes.at(fallback_view_controller_property_->getOptionInt()); + } + +private: + Ogre::SceneNode * m_sceneNode; + + bool m_fly_mode; + bool m_left_hand_mode; + bool m_removed_select; + + double m_pos_offset; + double m_boost; + + QStringList m_tool_classes; + // temporarily disabled + // std::vector m_tools; + // rviz_common::Tool *m_fallback_tool; + + QStringList m_view_controller_classes; + QString m_fallback_view_controller; + std::vector m_view_controller; + + rviz_common::properties::FloatProperty * step_length_property_; + rviz_common::properties::FloatProperty * boost_property_; + rviz_common::properties::BoolProperty * fly_property_; + rviz_common::properties::BoolProperty * left_hand_property_; + // temporarily disabled + // rviz_common::properties::EnumProperty *fallback_tool_property_; + rviz_common::properties::EnumProperty * fallback_view_controller_property_; + + // temporarily disabled + // void setFallbackToolProperty(); + void setFallbackViewControllerProperty(); +}; +} // namespace tier4_camera_view_rviz_plugin +#endif // BIRD_EYE_VIEW_TOOL_HPP_ diff --git a/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp b/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp new file mode 100644 index 0000000000000..9f4306a9d982a --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp @@ -0,0 +1,258 @@ +// Copyright 2023 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. +/* + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "third_person_view_controller.hpp" + +#include "rviz_common/display_context.hpp" +#include "rviz_common/properties/bool_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/properties/tf_frame_property.hpp" +#include "rviz_common/properties/vector_property.hpp" +#include "rviz_common/viewport_mouse_event.hpp" +#include "rviz_rendering/objects/shape.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace tier4_camera_view_rviz_plugin +{ +static const float PITCH_START = Ogre::Math::HALF_PI / 3; +static const float YAW_START = Ogre::Math::PI; +static const float DISTANCE_START = 30; +static const float FOCAL_SHAPE_SIZE_START = 0.05; +static const bool FOCAL_SHAPE_FIXED_SIZE = true; +static const char * TARGET_FRAME_START = "base_link"; + +// move camera up so the focal point appears in the lower image half +static const float CAMERA_OFFSET = 0.2; + +void ThirdPersonViewController::onInitialize() +{ + OrbitViewController::onInitialize(); + focal_shape_->setColor(0.0f, 1.0f, 1.0f, 0.5f); +} + +void ThirdPersonViewController::reset() +{ + yaw_property_->setFloat(YAW_START); + pitch_property_->setFloat(PITCH_START); + distance_property_->setFloat(DISTANCE_START); + focal_shape_size_property_->setFloat(FOCAL_SHAPE_SIZE_START); + focal_shape_fixed_size_property_->setBool(false); + updateFocalShapeSize(); + focal_point_property_->setVector(Ogre::Vector3::ZERO); +} + +std::pair ThirdPersonViewController::intersectGroundPlane(Ogre::Ray mouse_ray) +{ + // convert rays into reference frame + mouse_ray.setOrigin(target_scene_node_->convertWorldToLocalPosition(mouse_ray.getOrigin())); + mouse_ray.setDirection( + target_scene_node_->convertWorldToLocalOrientation(Ogre::Quaternion::IDENTITY) * + mouse_ray.getDirection()); + + Ogre::Plane ground_plane(Ogre::Vector3::UNIT_Z, 0); + + std::pair intersection = mouse_ray.intersects(ground_plane); + return std::make_pair(intersection.first, mouse_ray.getPoint(intersection.second)); +} + +void ThirdPersonViewController::handleMouseEvent(rviz_common::ViewportMouseEvent & event) +{ + if (event.shift()) { + setStatus("Left-Click: Move X/Y. Right-Click:: Zoom."); + } else { + setStatus( + "Left-Click: Rotate. Middle-Click: Move X/Y. Right-Click:: Move Z. " + " Shift: More options."); + } + + int32_t diff_x = 0; + int32_t diff_y = 0; + + bool moved = false; + if (event.type == QEvent::MouseButtonPress) { + focal_shape_->getRootNode()->setVisible(true); + moved = true; + } else if (event.type == QEvent::MouseButtonRelease) { + focal_shape_->getRootNode()->setVisible(false); + moved = true; + } else if (event.type == QEvent::MouseMove) { + diff_x = event.x - event.last_x; + diff_y = event.y - event.last_y; + moved = true; + } + + if (event.left() && !event.shift()) { + setCursor(Rotate3D); + yaw(diff_x * 0.005); + pitch(-diff_y * 0.005); + } else if (event.middle() || (event.left() && event.shift())) { + setCursor(MoveXY); + // handle mouse movement + int width = camera_->getViewport()->getActualWidth(); + int height = camera_->getViewport()->getActualHeight(); + + Ogre::Ray mouse_ray = camera_->getCameraToViewportRay( + event.x / static_cast(width), event.y / static_cast(height)); + + Ogre::Ray last_mouse_ray = camera_->getCameraToViewportRay( + event.last_x / static_cast(width), event.last_y / static_cast(height)); + + auto last_intersect_pair = intersectGroundPlane(last_mouse_ray); + auto intersect_pair = intersectGroundPlane(mouse_ray); + + if (last_intersect_pair.first && intersect_pair.first) { + Ogre::Vector3 motion = intersect_pair.second - last_intersect_pair.second; + + // When dragging near the horizon, the motion can get out of + // control. This throttles it to an arbitrary limit per mouse + // event. + float motion_distance_limit = 1; /*meter*/ + if (motion.length() > motion_distance_limit) { + motion.normalise(); + motion *= motion_distance_limit; + } + + focal_point_property_->add(motion); + emitConfigChanged(); + } + } else if (event.right()) { + setCursor(Zoom); + zoom(-diff_y * 0.1 * (distance_property_->getFloat() / 10.0f)); + } else { + setCursor(event.shift() ? MoveXY : Rotate3D); + } + + if (event.wheel_delta != 0) { + int diff = event.wheel_delta; + zoom(diff * 0.001 * distance_property_->getFloat()); + moved = true; + } + + if (moved) { + context_->queueRender(); + } +} + +void ThirdPersonViewController::mimic(rviz_common::ViewController * source_view) +{ + FramePositionTrackingViewController::mimic(source_view); + + target_frame_property_->setValue(TARGET_FRAME_START); + getNewTransform(); + + Ogre::Camera * source_camera = source_view->getCamera(); + + Ogre::Ray camera_dir_ray(source_camera->getRealPosition(), source_camera->getRealDirection()); + Ogre::Ray camera_down_ray(source_camera->getRealPosition(), -1.0f * source_camera->getRealUp()); + + auto camera_intersection = intersectGroundPlane(camera_dir_ray); + auto camera_down_intersection = intersectGroundPlane(camera_down_ray); + + if (camera_intersection.first && camera_down_intersection.first) { + // Set a focal point by intersecting with the ground plane from above. This will be possible + // if some part of the ground plane is visible in the view and the camera is above the z-plane. + float l_b = source_camera->getRealPosition().distance(camera_intersection.second); + float l_a = source_camera->getRealPosition().distance(camera_down_intersection.second); + + distance_property_->setFloat((l_b * l_a) / (CAMERA_OFFSET * l_b + l_a)); + Ogre::Vector3 position_offset = + source_camera->getRealUp() * distance_property_->getFloat() * CAMERA_OFFSET; + + camera_dir_ray.setOrigin(source_camera->getRealPosition() - position_offset); + auto new_focal_point = intersectGroundPlane(camera_dir_ray); + focal_point_property_->setVector(new_focal_point.second - reference_position_); + + calculatePitchYawFromPosition( + source_camera->getParentSceneNode()->getPosition() - position_offset); + } +} + +void ThirdPersonViewController::updateCamera() +{ + OrbitViewController::updateCamera(); + camera_->getParentSceneNode()->setPosition( + camera_->getParentSceneNode()->getPosition() + + camera_->getParentSceneNode()->getLocalAxes() * Ogre::Vector3::UNIT_Y * + distance_property_->getFloat() * CAMERA_OFFSET); +} + +void ThirdPersonViewController::updateTargetSceneNode() +{ + if (FramePositionTrackingViewController::getNewTransform()) { + target_scene_node_->setPosition(reference_position_); + Ogre::Radian ref_yaw = reference_orientation_.getRoll( + false); // OGRE camera frame looks along -Z, so they call rotation around Z "roll". + Ogre::Quaternion ref_yaw_quat(Ogre::Math::Cos(ref_yaw / 2), 0, 0, Ogre::Math::Sin(ref_yaw / 2)); + target_scene_node_->setOrientation(ref_yaw_quat); + + context_->queueRender(); + } +} + +void ThirdPersonViewController::lookAt(const Ogre::Vector3 & point) +{ + Ogre::Vector3 camera_position = camera_->getParentSceneNode()->getPosition(); + Ogre::Vector3 new_focal_point = + target_scene_node_->getOrientation().Inverse() * (point - target_scene_node_->getPosition()); + new_focal_point.z = 0; + distance_property_->setFloat(new_focal_point.distance(camera_position)); + focal_point_property_->setVector(new_focal_point); + + calculatePitchYawFromPosition(camera_position); +} + +} // namespace tier4_camera_view_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS( + tier4_camera_view_rviz_plugin::ThirdPersonViewController, rviz_common::ViewController) diff --git a/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.hpp b/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.hpp new file mode 100644 index 0000000000000..fef036ceccda3 --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/src/third_person_view_controller.hpp @@ -0,0 +1,94 @@ +// Copyright 2023 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. +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef THIRD_PERSON_VIEW_CONTROLLER_HPP_ +#define THIRD_PERSON_VIEW_CONTROLLER_HPP_ + +#include "rviz_default_plugins/view_controllers/orbit/orbit_view_controller.hpp" + +#include + +#include + +namespace Ogre +{ +class SceneNode; +} + +namespace tier4_camera_view_rviz_plugin +{ +class TfFrameProperty; + +/** + * \brief Like the orbit view controller, but focal point moves only in the x-y plane. + */ +class ThirdPersonViewController : public rviz_default_plugins::view_controllers::OrbitViewController +{ + Q_OBJECT + +public: + void onInitialize() override; + + void handleMouseEvent(rviz_common::ViewportMouseEvent & evt) override; + + void lookAt(const Ogre::Vector3 & point) override; + + void reset() override; + + /** @brief Configure the settings of this view controller to give, + * as much as possible, a similar view as that given by the + * @a source_view. + * + * @a source_view must return a valid @c Ogre::Camera* from getCamera(). */ + void mimic(rviz_common::ViewController * source_view) override; + +protected: + void updateCamera() override; + + void updateTargetSceneNode() override; + + std::pair intersectGroundPlane(Ogre::Ray mouse_ray); +}; + +} // namespace tier4_camera_view_rviz_plugin + +#endif // THIRD_PERSON_VIEW_CONTROLLER_HPP_ diff --git a/common/tier4_camera_view_rviz_plugin/src/third_person_view_tool.cpp b/common/tier4_camera_view_rviz_plugin/src/third_person_view_tool.cpp new file mode 100644 index 0000000000000..09c7fe0c2677a --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/src/third_person_view_tool.cpp @@ -0,0 +1,173 @@ +// Copyright 2023-2024 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. +/* + * UOS-ROS packages - Robot Operating System code by the University of Osnabrück + * Copyright (C) 2014 University of Osnabrück + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted + * provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this list of conditions + * and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, this list of + * conditions and the following disclaimer in the documentation and/or other materials provided with + * the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR + * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND + * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY + * WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * route_set_tool.cpp + * + * Author: Henning Deeken {hdeeken@uos.de} + * + * + */ + +#include "third_person_view_tool.hpp" + +#include "rviz_common/display_context.hpp" +#include "rviz_common/view_manager.hpp" + +#include + +namespace tier4_camera_view_rviz_plugin +{ +ThirdPersonViewTool::ThirdPersonViewTool() +{ + shortcut_key_ = 'o'; +} + +ThirdPersonViewTool::~ThirdPersonViewTool() +{ +} + +void ThirdPersonViewTool::onInitialize() +{ + setName("Third Person View"); + + step_length_property_ = new rviz_common::properties::FloatProperty( + "Step Length", 0.1, "The length by with the position is updated on each step.", + getPropertyContainer(), SLOT(setOffset()), this); + + boost_property_ = new rviz_common::properties::FloatProperty( + "Boost Property", 0.5, "Gives the boost factor which is applied if pressing shift.", + getPropertyContainer(), SLOT(setBoost()), this); + + fly_property_ = new rviz_common::properties::BoolProperty( + "Fly Mode", false, "In fly mode it is possible to move along the z axis as well.", + getPropertyContainer(), SLOT(setFlyMode()), this); + + left_hand_property_ = new rviz_common::properties::BoolProperty( + "Left Hand Mode", false, "In left hand mode one uses the arrows to move around", + getPropertyContainer(), SLOT(setLeftHandMode()), this); + + // temporarily disabled + // fallback_tool_property_ = new rviz_common::properties::EnumProperty( + // "Fallback Tool", QString("tier4_camera_view_rviz_plugin/ThirdPersonViewTool"), + // "Determines to which tool the control switches, if the tool is deactivated.", + // getPropertyContainer(), SLOT(setFallbackTool()), this); + fallback_view_controller_property_ = new rviz_common::properties::EnumProperty( + "Fallback ViewController", QString("tier4_camera_view_rviz_plugin/ThirdPersonView"), + "Determines to which view controller the control switches, if the tool is deactivated.", + getPropertyContainer(), SLOT(setFallbackViewController()), this); + + m_pos_offset = 0.1; + m_boost = 0.5; + m_fly_mode = false; + m_left_hand_mode = false; + + // temporarily disabled + // setFallbackToolProperty(); + setFallbackViewControllerProperty(); +} + +void ThirdPersonViewTool::setFallbackViewControllerProperty() +{ + fallback_view_controller_property_->clearOptions(); + m_view_controller_classes.clear(); + + m_view_controller_classes = context_->getViewManager()->getDeclaredClassIdsFromFactory(); + + for (int i = 0; i < m_view_controller_classes.size(); i++) { + if (m_view_controller_classes[i] != QString("tier4_camera_view_rviz_plugin/ThirdPersonView")) { + fallback_view_controller_property_->addOption(m_view_controller_classes[i], i); + m_view_controller.push_back(context_->getViewManager()->getViewAt(i)); + } + } + + fallback_view_controller_property_->show(); + setFallbackViewController(); +} + +// temporarily disabled +// void ThirdPersonViewTool::setFallbackToolProperty() +// { +// fallback_tool_property_->clearOptions(); +// m_tools.clear(); + +// m_tool_classes = context_->getToolManager()->getToolClasses(); + +// for (int i = 0; i < m_tool_classes.size(); i++) { +// if (m_tool_classes[i] != getClassId()) { +// fallback_tool_property_->addOption(m_tool_classes[i], i); +// m_tools.push_back(context_->getToolManager()->getTool(i)); +// } +// } + +// fallback_tool_property_->show(); +// setFallbackTool(); +// } + +void ThirdPersonViewTool::activate() +{ + context_->getViewManager()->setCurrentViewControllerType( + QString("tier4_camera_view_rviz_plugin/ThirdPersonView")); + context_->getViewManager()->getCurrent()->reset(); + + // temporarily disabled + // setFallbackToolProperty(); + setFallbackViewControllerProperty(); +} + +void ThirdPersonViewTool::deactivate() +{ +} + +int ThirdPersonViewTool::processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel) +{ + return 0; +} + +int ThirdPersonViewTool::processMouseEvent(rviz_common::ViewportMouseEvent & event) +{ + if (event.panel->getViewController()) { + event.panel->getViewController()->handleMouseEvent(event); + setCursor(event.panel->getViewController()->getCursor()); + } + return 0; +} + +} // namespace tier4_camera_view_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS(tier4_camera_view_rviz_plugin::ThirdPersonViewTool, rviz_common::Tool) diff --git a/common/tier4_camera_view_rviz_plugin/src/third_person_view_tool.hpp b/common/tier4_camera_view_rviz_plugin/src/third_person_view_tool.hpp new file mode 100644 index 0000000000000..37d0db13b4629 --- /dev/null +++ b/common/tier4_camera_view_rviz_plugin/src/third_person_view_tool.hpp @@ -0,0 +1,144 @@ +// Copyright 2023 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. +/* + * UOS-ROS packages - Robot Operating System code by the University of Osnabrück + * Copyright (C) 2014 University of Osnabrück + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted + * provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this list of conditions + * and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, this list of + * conditions and the following disclaimer in the documentation and/or other materials provided with + * the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR + * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND + * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * route_set_tool.h + * + * Author: Henning Deeken + * + */ + +#ifndef THIRD_PERSON_VIEW_TOOL_HPP_ +#define THIRD_PERSON_VIEW_TOOL_HPP_ + +#include "rviz_common/properties/bool_property.hpp" +#include "rviz_common/properties/enum_property.hpp" +#include "rviz_common/properties/float_property.hpp" +#include "rviz_common/render_panel.hpp" +#include "rviz_common/tool.hpp" +#include "rviz_common/viewport_mouse_event.hpp" +#include "third_person_view_controller.hpp" + +#include +#include +#include + +#include +#include + +/** + * + *@class ThirdPersonViewTool + * + *@brief Implements a rviz tool that allows to navigate in a ego-shooter mode. + */ + +namespace tier4_camera_view_rviz_plugin +{ + +class FPSMotionConfigWidget; +class ThirdPersonViewTool : public rviz_common::Tool +{ + Q_OBJECT + +public: + ThirdPersonViewTool(); + ~ThirdPersonViewTool(); + virtual void onInitialize(); + virtual void activate(); + virtual void deactivate(); + + virtual int processKeyEvent(QKeyEvent * event, rviz_common::RenderPanel * panel); + virtual int processMouseEvent(rviz_common::ViewportMouseEvent & event); + +private Q_SLOTS: + void setOffset() { m_pos_offset = static_cast(step_length_property_->getFloat()); } + void setBoost() + { + if (boost_property_->getFloat() < 0.0) { + m_boost = 0.0; + } else if (boost_property_->getFloat() > 1.0) { + m_boost = 1.0; + } else { + m_boost = static_cast(boost_property_->getFloat()); + } + } + + void setFlyMode() { m_fly_mode = fly_property_->getBool(); } + void setLeftHandMode() { m_left_hand_mode = left_hand_property_->getBool(); } + // temporarily disabled + // void setFallbackTool() { m_fallback_tool = m_tools.at(fallback_tool_property_->getOptionInt()); + // } + void setFallbackViewController() + { + m_fallback_view_controller = + m_view_controller_classes.at(fallback_view_controller_property_->getOptionInt()); + } + +private: + Ogre::SceneNode * m_sceneNode; + + bool m_fly_mode; + bool m_left_hand_mode; + bool m_removed_select; + + double m_pos_offset; + double m_boost; + + QStringList m_tool_classes; + // temporarily disabled + // std::vector m_tools; + // rviz_common::Tool *m_fallback_tool; + + QStringList m_view_controller_classes; + QString m_fallback_view_controller; + std::vector m_view_controller; + + rviz_common::properties::FloatProperty * step_length_property_; + rviz_common::properties::FloatProperty * boost_property_; + rviz_common::properties::BoolProperty * fly_property_; + rviz_common::properties::BoolProperty * left_hand_property_; + // temporarily disabled + // rviz_common::properties::EnumProperty *fallback_tool_property_; + rviz_common::properties::EnumProperty * fallback_view_controller_property_; + + // temporarily disabled + // void setFallbackToolProperty(); + void setFallbackViewControllerProperty(); +}; +} // namespace tier4_camera_view_rviz_plugin +#endif // THIRD_PERSON_VIEW_TOOL_HPP_ diff --git a/common/tier4_logging_level_configure_rviz_plugin/README.md b/common/tier4_logging_level_configure_rviz_plugin/README.md index c3523ed802205..8df17a82e3de7 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/README.md +++ b/common/tier4_logging_level_configure_rviz_plugin/README.md @@ -3,3 +3,7 @@ This package provides an rviz_plugin that can easily change the logger level of each node ![tier4_logging_level_configure_rviz_plugin](tier4_logging_level_configure_rviz_plugin.png) + +This plugin dispatches services to the "logger name" associated with "nodes" specified in YAML, adjusting the logger level. + +As of November 2023, in ROS 2 Humble, users are required to initiate a service server in the node to use this feature. (This might be integrated into ROS standards in the future.) For easy service server generation, you can use the [LoggerLevelConfigure](https://github.com/autowarefoundation/autoware.universe/blob/main/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/logger_level_configure.hpp) utility. diff --git a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml index 97c3104242026..23a5123d8d8e2 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml +++ b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml @@ -1,63 +1,87 @@ # logger_config.yaml +# ============================================================ +# localization +# ============================================================ +Localization: + ndt_scan_matcher: + - node_name: /localization/pose_estimator/ndt_scan_matcher + logger_name: localization.pose_estimator.ndt_scan_matcher + + gyro_odometer: + - node_name: /localization/twist_estimator/gyro_odometer + logger_name: localization.twist_estimator.gyro_odometer + + pose_initializer: + - node_name: /localization/util/pose_initializer_node + logger_name: localization.util.pose_initializer_node + + ekf_localizer: + - node_name: /localization/pose_twist_fusion_filter/ekf_localizer + logger_name: localization.pose_twist_fusion_filter.ekf_localizer + + localization_error_monitor: + - node_name: /localization/localization_error_monitor + logger_name: localization.localization_error_monitor + # ============================================================ # planning # ============================================================ +Planning: + behavior_path_planner: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: tier4_autoware_utils -behavior_path_planner: - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: tier4_autoware_utils - -behavior_path_planner_avoidance: - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance - -behavior_path_planner_lane_change: - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner - logger_name: lane_change - -behavior_velocity_planner: - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner - logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner - logger_name: tier4_autoware_utils - -behavior_velocity_planner_intersection: - - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner - logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.intersection - -motion_obstacle_avoidance: - - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner - logger_name: planning.scenario_planning.lane_driving.motion_planning.obstacle_avoidance_planner - - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner - logger_name: tier4_autoware_utils - -motion_velocity_smoother: - - node_name: /planning/scenario_planning/motion_velocity_smoother - logger_name: planning.scenario_planning.motion_velocity_smoother - - node_name: /planning/scenario_planning/motion_velocity_smoother - logger_name: tier4_autoware_utils + behavior_path_planner_avoidance: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance + + behavior_path_planner_lane_change: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: lane_change + + behavior_velocity_planner: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: tier4_autoware_utils + + behavior_velocity_planner_intersection: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.intersection + + motion_obstacle_avoidance: + - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner + logger_name: planning.scenario_planning.lane_driving.motion_planning.obstacle_avoidance_planner + - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner + logger_name: tier4_autoware_utils + + motion_velocity_smoother: + - node_name: /planning/scenario_planning/motion_velocity_smoother + logger_name: planning.scenario_planning.motion_velocity_smoother + - node_name: /planning/scenario_planning/motion_velocity_smoother + logger_name: tier4_autoware_utils # ============================================================ # control # ============================================================ +Control: + lateral_controller: + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: control.trajectory_follower.controller_node_exe.lateral_controller + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: tier4_autoware_utils + + longitudinal_controller: + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: control.trajectory_follower.controller_node_exe.longitudinal_controller + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: tier4_autoware_utils -lateral_controller: - - node_name: /control/trajectory_follower/controller_node_exe - logger_name: control.trajectory_follower.controller_node_exe.lateral_controller - - node_name: /control/trajectory_follower/controller_node_exe - logger_name: tier4_autoware_utils - -longitudinal_controller: - - node_name: /control/trajectory_follower/controller_node_exe - logger_name: control.trajectory_follower.controller_node_exe.longitudinal_controller - - node_name: /control/trajectory_follower/controller_node_exe - logger_name: tier4_autoware_utils - -vehicle_cmd_gate: - - node_name: /control/vehicle_cmd_gate - logger_name: control.vehicle_cmd_gate - - node_name: /control/vehicle_cmd_gate - logger_name: tier4_autoware_utils + vehicle_cmd_gate: + - node_name: /control/vehicle_cmd_gate + logger_name: control.vehicle_cmd_gate + - node_name: /control/vehicle_cmd_gate + logger_name: tier4_autoware_utils diff --git a/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp b/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp index 4d9b81ffb86bf..37d70b494c74e 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp +++ b/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp @@ -34,7 +34,21 @@ namespace rviz_plugin { - +struct LoggerInfo +{ + QString node_name; + QString logger_name; +}; +struct ButtonInfo +{ + QString button_name; + std::vector logger_info_vec; +}; +struct LoggerNamespaceInfo +{ + QString ns; // group namespace + std::vector button_info_vec; +}; class LoggingLevelConfigureRvizPlugin : public rviz_common::Panel { Q_OBJECT // This macro is needed for Qt to handle slots and signals @@ -48,8 +62,7 @@ class LoggingLevelConfigureRvizPlugin : public rviz_common::Panel QMap buttonGroups_; rclcpp::Node::SharedPtr raw_node_; - // node_logger_map_[button_name] = {node_name, logger_name} - std::map>> node_logger_map_; + std::vector display_info_vec_; // client_map_[node_name] = service_client std::unordered_map::SharedPtr> @@ -62,6 +75,9 @@ class LoggingLevelConfigureRvizPlugin : public rviz_common::Panel int getMaxModuleNameWidth(QLabel * containerLabel); void setLoggerNodeMap(); + ButtonInfo getButtonInfoFromNamespace(const QString & ns); + std::vector getNodeLoggerNameFromButtonName(const QString button_name); + private Q_SLOTS: void onButtonClick(QPushButton * button, const QString & name, const QString & level); void updateButtonColors( diff --git a/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp b/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp index b15c0f0f735fa..72ecf361c96d5 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp +++ b/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp @@ -14,6 +14,7 @@ #include "yaml-cpp/yaml.h" +#include #include #include #include @@ -36,56 +37,72 @@ void LoggingLevelConfigureRvizPlugin::onInitialize() setLoggerNodeMap(); - QVBoxLayout * layout = new QVBoxLayout; + QVBoxLayout * mainLayout = new QVBoxLayout; QStringList levels = {"DEBUG", "INFO", "WARN", "ERROR", "FATAL"}; constexpr int height = 20; - for (const auto & item : node_logger_map_) { - const auto & target_node_name = item.first; - - QHBoxLayout * hLayout = new QHBoxLayout; - - // Create a QLabel to display the node name. - QLabel * label = new QLabel(target_node_name); - label->setFixedHeight(height); // Set fixed height for the button - label->setFixedWidth(getMaxModuleNameWidth(label)); - - hLayout->addWidget(label); // Add the QLabel to the hLayout. - - QButtonGroup * group = new QButtonGroup(this); - for (const QString & level : levels) { - QPushButton * btn = new QPushButton(level); - btn->setFixedHeight(height); // Set fixed height for the button - hLayout->addWidget(btn); // Add each QPushButton to the hLayout. - group->addButton(btn); - button_map_[target_node_name][level] = btn; - connect(btn, &QPushButton::clicked, this, [this, btn, target_node_name, level]() { - this->onButtonClick(btn, target_node_name, level); - }); + + // Iterate over the namespaces + for (const auto & ns_group_info : display_info_vec_) { + // Create a group box for each namespace + QGroupBox * groupBox = new QGroupBox(ns_group_info.ns); + QVBoxLayout * groupLayout = new QVBoxLayout; + + // Iterate over the node/logger pairs within this namespace + for (const auto & button_info : ns_group_info.button_info_vec) { + const auto & button_name = button_info.button_name; + + QHBoxLayout * hLayout = new QHBoxLayout; + + // Create a QLabel to display the node name + QLabel * label = new QLabel(button_name); + label->setFixedHeight(height); + label->setFixedWidth(getMaxModuleNameWidth(label)); + + hLayout->addWidget(label); + + // Create a button group for each node + QButtonGroup * buttonGroup = new QButtonGroup(this); + + // Create buttons for each logging level + for (const QString & level : levels) { + QPushButton * button = new QPushButton(level); + button->setFixedHeight(height); + hLayout->addWidget(button); + buttonGroup->addButton(button); + button_map_[button_name][level] = button; + connect(button, &QPushButton::clicked, this, [this, button, button_name, level]() { + this->onButtonClick(button, button_name, level); + }); + } + + // Set the "INFO" button as checked by default and change its color + updateButtonColors(button_name, button_map_[button_name]["INFO"], "INFO"); + + buttonGroups_[button_name] = buttonGroup; + groupLayout->addLayout(hLayout); // Add the horizontal layout to the group layout } - // Set the "INFO" button as checked by default and change its color. - updateButtonColors(target_node_name, button_map_[target_node_name]["INFO"], "INFO"); - buttonGroups_[target_node_name] = group; - layout->addLayout(hLayout); + groupBox->setLayout(groupLayout); // Set the group layout to the group box + mainLayout->addWidget(groupBox); // Add the group box to the main layout } - // Create a QWidget to hold the layout. + // Create a QWidget to hold the main layout QWidget * containerWidget = new QWidget; - containerWidget->setLayout(layout); + containerWidget->setLayout(mainLayout); - // Create a QScrollArea to make the layout scrollable. + // Create a QScrollArea to make the layout scrollable QScrollArea * scrollArea = new QScrollArea; scrollArea->setWidget(containerWidget); scrollArea->setWidgetResizable(true); - // Set the QScrollArea as the layout of the main widget. - QVBoxLayout * mainLayout = new QVBoxLayout; - mainLayout->addWidget(scrollArea); - setLayout(mainLayout); + // Set the QScrollArea as the layout of the main widget + QVBoxLayout * scrollLayout = new QVBoxLayout; + scrollLayout->addWidget(scrollArea); + setLayout(scrollLayout); - // set up service clients + // Setup service clients const auto & nodes = getNodeList(); for (const QString & node : nodes) { const auto client = raw_node_->create_client( @@ -99,9 +116,10 @@ int LoggingLevelConfigureRvizPlugin::getMaxModuleNameWidth(QLabel * label) { int max_width = 0; QFontMetrics metrics(label->font()); - for (const auto & item : node_logger_map_) { - const auto & target_module_name = item.first; - max_width = std::max(metrics.horizontalAdvance(target_module_name), max_width); + for (const auto & ns_info : display_info_vec_) { + for (const auto & b : ns_info.button_info_vec) { + max_width = std::max(metrics.horizontalAdvance(b.button_name), max_width); + } } return max_width; } @@ -110,11 +128,12 @@ int LoggingLevelConfigureRvizPlugin::getMaxModuleNameWidth(QLabel * label) QStringList LoggingLevelConfigureRvizPlugin::getNodeList() { QStringList nodes; - for (const auto & item : node_logger_map_) { - const auto & node_logger_vec = item.second; - for (const auto & node_logger_pair : node_logger_vec) { - if (!nodes.contains(node_logger_pair.first)) { - nodes.append(node_logger_pair.first); + for (const auto & d : display_info_vec_) { + for (const auto & b : d.button_info_vec) { + for (const auto & info : b.logger_info_vec) { + if (!nodes.contains(info.node_name)) { + nodes.append(info.node_name); + } } } } @@ -132,16 +151,15 @@ void LoggingLevelConfigureRvizPlugin::onButtonClick( << std::string(future.get()->success ? "success!" : "failed...") << std::endl; }; - for (const auto & node_logger_map : node_logger_map_[target_module_name]) { - const auto node_name = node_logger_map.first; - const auto logger_name = node_logger_map.second; + const auto node_logger_vec = getNodeLoggerNameFromButtonName(target_module_name); + for (const auto & data : node_logger_vec) { const auto req = std::make_shared(); - req->logger_name = logger_name.toStdString(); + req->logger_name = data.logger_name.toStdString(); req->level = level.toStdString(); std::cerr << "logger level of " << req->logger_name << " is set to " << req->level << std::endl; - client_map_[node_name]->async_send_request(req, callback); + client_map_[data.node_name]->async_send_request(req, callback); } updateButtonColors( @@ -197,14 +215,42 @@ void LoggingLevelConfigureRvizPlugin::setLoggerNodeMap() YAML::Node config = YAML::LoadFile(filename); for (YAML::const_iterator it = config.begin(); it != config.end(); ++it) { - const auto key = QString::fromStdString(it->first.as()); - const YAML::Node values = it->second; - for (size_t i = 0; i < values.size(); i++) { - const auto node_name = QString::fromStdString(values[i]["node_name"].as()); - const auto logger_name = QString::fromStdString(values[i]["logger_name"].as()); - node_logger_map_[key].push_back({node_name, logger_name}); + const auto ns = QString::fromStdString(it->first.as()); + const YAML::Node ns_config = it->second; + + LoggerNamespaceInfo display_data; + display_data.ns = ns; + + for (YAML::const_iterator ns_it = ns_config.begin(); ns_it != ns_config.end(); ++ns_it) { + const auto key = QString::fromStdString(ns_it->first.as()); + ButtonInfo button_data; + button_data.button_name = key; + const YAML::Node values = ns_it->second; + for (size_t i = 0; i < values.size(); i++) { + LoggerInfo data; + data.node_name = QString::fromStdString(values[i]["node_name"].as()); + data.logger_name = QString::fromStdString(values[i]["logger_name"].as()); + button_data.logger_info_vec.push_back(data); + } + display_data.button_info_vec.push_back(button_data); + } + display_info_vec_.push_back(display_data); + } +} + +std::vector LoggingLevelConfigureRvizPlugin::getNodeLoggerNameFromButtonName( + const QString button_name) +{ + for (const auto & ns_level : display_info_vec_) { + for (const auto & button : ns_level.button_info_vec) { + if (button.button_name == button_name) { + return button.logger_info_vec; + } } } + RCLCPP_ERROR( + raw_node_->get_logger(), "Failed to find target name: %s", button_name.toStdString().c_str()); + return {}; } } // namespace rviz_plugin diff --git a/common/tier4_planning_rviz_plugin/package.xml b/common/tier4_planning_rviz_plugin/package.xml index 5dfa3605cfcad..4af4a371ef651 100644 --- a/common/tier4_planning_rviz_plugin/package.xml +++ b/common/tier4_planning_rviz_plugin/package.xml @@ -5,7 +5,6 @@ 0.1.0 The tier4_planning_rviz_plugin package Yukihiro Saito - Yutaka Shimizu Takayuki Murooka Apache License 2.0 diff --git a/common/tvm_utility/.gitignore b/common/tvm_utility/.gitignore index a09bb7234b379..e69de29bb2d1d 100644 --- a/common/tvm_utility/.gitignore +++ b/common/tvm_utility/.gitignore @@ -1,2 +0,0 @@ -artifacts/**/*.jpg -data/ diff --git a/common/tvm_utility/CMakeLists.txt b/common/tvm_utility/CMakeLists.txt index b49f141d9e2e4..c0a0d7385f615 100644 --- a/common/tvm_utility/CMakeLists.txt +++ b/common/tvm_utility/CMakeLists.txt @@ -29,15 +29,19 @@ set(TVM_UTILITY_NODE_LIB_HEADERS ament_auto_add_library(${PROJECT_NAME} SHARED ${TVM_UTILITY_NODE_LIB_HEADERS}) set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) -if(BUILD_TESTING) +set(BUILD_EXAMPLE OFF CACHE BOOL "enable build yolo_v2_tiny") + +if(BUILD_TESTING OR BUILD_EXAMPLE) + find_package(OpenCV REQUIRED) + set(tvm_runtime_DIR ${tvm_vendor_DIR}) + find_package(tvm_runtime CONFIG REQUIRED) # Get target backend set(${PROJECT_NAME}_BACKEND llvm CACHE STRING "${PROJECT_NAME} neural network backend") +endif() +if(BUILD_TESTING) # compile each folder inside test/ as a test case find_package(ament_cmake_gtest REQUIRED) - find_package(OpenCV REQUIRED) - set(tvm_runtime_DIR ${tvm_vendor_DIR}) - find_package(tvm_runtime CONFIG REQUIRED) set(TEST_ARTIFACTS "${CMAKE_CURRENT_LIST_DIR}/artifacts") file(GLOB TEST_CASES test/*) @@ -47,17 +51,11 @@ if(BUILD_TESTING) endif() # the folder name becomes the test case name file(RELATIVE_PATH TEST_CASE_NAME ${CMAKE_CURRENT_LIST_DIR}/test ${TEST_FOLDER}) - # Get neural network. set(NN_DEPENDENCY "") - get_neural_network(${TEST_CASE_NAME} ${${PROJECT_NAME}_BACKEND} NN_DEPENDENCY) + get_neural_network(${TEST_CASE_NAME}_${CMAKE_SYSTEM_PROCESSOR} ${${PROJECT_NAME}_BACKEND} NN_DEPENDENCY) if(NOT NN_DEPENDENCY STREQUAL "") - if(TEST_CASE_NAME STREQUAL "yolo_v2_tiny" AND - NOT EXISTS ${TEST_ARTIFACTS}/yolo_v2_tiny/test_image_0.jpg) - message(WARNING "Missing image artifact for yolo_v2_tiny, skipping test") - continue() - endif() # add all cpp files in the folder to the target file(GLOB TEST_CASE_SOURCES ${TEST_FOLDER}/*.cpp) ament_add_gtest(${TEST_CASE_NAME} ${TEST_CASE_SOURCES}) @@ -75,7 +73,7 @@ if(BUILD_TESTING) target_include_directories("${TEST_CASE_NAME}" SYSTEM PUBLIC "${OpenCV_INCLUDE_DIRS}" "${tvm_utility_FOUND_INCLUDE_DIRS}" - "data/models" + "data/models/${TEST_CASE_NAME}_${CMAKE_SYSTEM_PROCESSOR}" "include" ) @@ -93,5 +91,61 @@ if(BUILD_TESTING) endforeach() endif() +if(BUILD_EXAMPLE) + # compile each folder inside example/ as an example + find_package(rclcpp REQUIRED) + + set(EXAMPLE_ARTIFACTS "${CMAKE_CURRENT_LIST_DIR}/artifacts") + file(GLOB EXAMPLE_CASES example/*) + foreach(EXAMPLE_FOLDER ${EXAMPLE_CASES}) + if(NOT IS_DIRECTORY ${EXAMPLE_FOLDER}) + continue() + endif() + # the folder name becomes the example name + file(RELATIVE_PATH EXAMPLE_NAME ${CMAKE_CURRENT_LIST_DIR}/example ${EXAMPLE_FOLDER}) + # Get neural network. + set(NN_DEPENDENCY "") + get_neural_network(${EXAMPLE_NAME} ${${PROJECT_NAME}_BACKEND} NN_DEPENDENCY) + + if(NOT NN_DEPENDENCY STREQUAL "") + if(EXAMPLE_NAME STREQUAL "yolo_v2_tiny" AND + NOT EXISTS ${EXAMPLE_ARTIFACTS}/yolo_v2_tiny/test_image_0.jpg) + message(WARNING "Missing image artifact for yolo_v2_tiny, skipping example") + continue() + endif() + # add all cpp files in the folder to the target + file(GLOB EXAMPLE_SOURCES ${EXAMPLE_FOLDER}/*.cpp) + ament_auto_add_executable(${EXAMPLE_NAME} ${EXAMPLE_SOURCES}) + ament_target_dependencies(${EXAMPLE_NAME} + "ament_index_cpp" + "tvm_vendor" + "rclcpp" + ) + add_dependencies(${EXAMPLE_NAME} ${NN_DEPENDENCY}) + + target_link_libraries("${EXAMPLE_NAME}" + "${OpenCV_LIBRARIES}" + "${tvm_runtime_LIBRARIES}" + ) + + target_include_directories("${EXAMPLE_NAME}" SYSTEM PUBLIC + "${OpenCV_INCLUDE_DIRS}" + "${tvm_utility_FOUND_INCLUDE_DIRS}" + "data/models" + "include" + ) + + else() + message(WARNING "No model is generated for ${EXAMPLE_FOLDER} example") + endif() + + endforeach() +endif() + list(APPEND ${PROJECT_NAME}_CONFIG_EXTRAS "${PROJECT_NAME}-extras.cmake") -ament_auto_package() +# ament_auto_package() +ament_auto_package( + INSTALL_TO_SHARE + launch + config + artifacts) diff --git a/common/tvm_utility/README.md b/common/tvm_utility/README.md index 7d4874d5ed89a..4751428353886 100644 --- a/common/tvm_utility/README.md +++ b/common/tvm_utility/README.md @@ -41,7 +41,7 @@ The earliest supported version depends on each package making use of the inferen #### Models -Dependent packages are expected to use the `get_neural_network` cmake function from this package in order to get the compiled TVM models. +Dependent packages are expected to use the `get_neural_network` cmake function from this package in order to build proper external dependency. ### Error detection and handling @@ -50,76 +50,55 @@ error description. ### Neural Networks Provider -This package also provides a utility to get pre-compiled neural networks to packages using them for their inference. - The neural networks are compiled as part of the [Model Zoo](https://github.com/autowarefoundation/modelzoo/) CI pipeline and saved to an S3 bucket. -This package exports cmake variables and functions for ease of access to those neural networks. The `get_neural_network` function creates an abstraction for the artifact management. -The artifacts are saved under the source directory of the package making use of the function; under "data/". -Priority is given to user-provided files, under "data/user/${MODEL_NAME}/". -If there are no user-provided files, the function tries to reuse previously-downloaded artifacts. -If there are no previously-downloaded artifacts, and if the `DOWNLOAD_ARTIFACTS` cmake variable is set, they will be downloaded from the bucket. -Otherwise, nothing happens. +Users should check if model configuration header file is under "data/user/${MODEL_NAME}/". Otherwise, nothing happens and compilation of the package will be skipped. The structure inside of the source directory of the package making use of the function is as follow: ```{text} . ├── data -│ ├── downloads -│ │ ├── ${MODEL 1}-${ARCH 1}-{BACKEND 1}-{VERSION 1}.tar.gz -│ │ ├── ... -│ │ └── ${MODEL ...}-${ARCH ...}-{BACKEND ...}-{VERSION ...}.tar.gz -│ ├── models -│ │ ├── ${MODEL 1} -│ │ │ ├── ... -│ │ │ └── inference_engine_tvm_config.hpp -│ │ ├── ... -│ │ └── ${MODEL ...} -│ │ └── ... -│ └── user +│ └── models │ ├── ${MODEL 1} -│ │ ├── deploy_graph.json -│ │ ├── deploy_lib.so -│ │ ├── deploy_param.params │ │ └── inference_engine_tvm_config.hpp │ ├── ... │ └── ${MODEL ...} │ └── ... ``` -The `inference_engine_tvm_config.hpp` file needed for compilation by dependent packages is made available under "data/models/${MODEL_NAME}/inference_engine_tvm_config.hpp". +The `inference_engine_tvm_config.hpp` file needed for compilation by dependent packages should be available under "data/models/${MODEL_NAME}/inference_engine_tvm_config.hpp". Dependent packages can use the cmake `add_dependencies` function with the name provided in the `DEPENDENCY` output parameter of `get_neural_network` to ensure this file is created before it gets used. The other `deploy_*` files are installed to "models/${MODEL_NAME}/" under the `share` directory of the package. -The target version to be downloaded can be overwritten by setting the `MODELZOO_VERSION` cmake variable. - -#### Assumptions / Known limits - -If several packages make use of the same neural network, it will be downloaded once per package. - -In case a requested artifact doesn't exist in the S3 bucket, the error message from ExternalProject is not explicit enough for the user to understand what went wrong. +The other model files should be stored in autoware_data folder under package folder with the structure: -In case the user manually sets `MODELZOO_VERSION` to "latest", the archive will not be re-downloaded when it gets updated in the S3 bucket (it is not a problem for tagged versions as they are not expected to be updated). +```{text} +$HOME/autoware_data +| └──${package} +| └──models +| ├── ${MODEL 1} +| | ├── deploy_graph.json +| | ├── deploy_lib.so +| | └── deploy_param.params +| ├── ... +| └── ${MODEL ...} +| └── ... +``` #### Inputs / Outputs -Inputs: - -- `DOWNLOAD_ARTIFACTS` cmake variable; needs to be set to enable downloading the artifacts -- `MODELZOO_VERSION` cmake variable; can be used to overwrite the default target version of downloads - Outputs: -- `get_neural_network` cmake function; can be used to get a neural network compiled for a specific backend +- `get_neural_network` cmake function; create proper external dependency for a package with use of the model provided by the user. In/Out: - The `DEPENDENCY` argument of `get_neural_network` can be checked for the outcome of the function. - It is an empty string when the neural network couldn't be made available. + It is an empty string when the neural network wasn't provided by the user. ## Security considerations diff --git a/common/tvm_utility/config/yolo_v2_tiny_example.param.yaml b/common/tvm_utility/config/yolo_v2_tiny_example.param.yaml new file mode 100644 index 0000000000000..b63e4a99f97f2 --- /dev/null +++ b/common/tvm_utility/config/yolo_v2_tiny_example.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + image_filename: $(find-pkg-share tvm_utility)/artifacts/yolo_v2_tiny/test_image_0.jpg + label_filename: $(find-pkg-share tvm_utility)/artifacts/yolo_v2_tiny/labels.txt + anchor_filename: $(find-pkg-share tvm_utility)/artifacts/yolo_v2_tiny/anchors.csv + data_path: $(env HOME)/autoware_data diff --git a/common/tvm_utility/data/models/abs_model_aarch64/deploy_graph.json b/common/tvm_utility/data/models/abs_model_aarch64/deploy_graph.json new file mode 100644 index 0000000000000..b226c01747dca --- /dev/null +++ b/common/tvm_utility/data/models/abs_model_aarch64/deploy_graph.json @@ -0,0 +1,36 @@ +{ + "nodes": [ + { + "op": "null", + "name": "a", + "inputs": [] + }, + { + "op": "tvm_op", + "name": "tvmgen_default_fused_abs", + "attrs": { + "num_outputs": "1", + "num_inputs": "1", + "flatten_data": "0", + "func_name": "tvmgen_default_fused_abs", + "hash": "1be44995aa501758" + }, + "inputs": [[0, 0, 0]] + } + ], + "arg_nodes": [0], + "heads": [[1, 0, 0]], + "attrs": { + "dltype": ["list_str", ["float32", "float32"]], + "device_index": ["list_int", [1, 1]], + "storage_id": ["list_int", [0, 1]], + "shape": [ + "list_shape", + [ + [2, 2], + [2, 2] + ] + ] + }, + "node_row_ptr": [0, 1, 2] +} diff --git a/common/tvm_utility/data/models/abs_model_aarch64/deploy_lib.so b/common/tvm_utility/data/models/abs_model_aarch64/deploy_lib.so new file mode 100755 index 0000000000000..e1ad7cebad734 Binary files /dev/null and b/common/tvm_utility/data/models/abs_model_aarch64/deploy_lib.so differ diff --git a/common/tvm_utility/data/models/abs_model_aarch64/deploy_param.params b/common/tvm_utility/data/models/abs_model_aarch64/deploy_param.params new file mode 100644 index 0000000000000..1011def01ed82 Binary files /dev/null and b/common/tvm_utility/data/models/abs_model_aarch64/deploy_param.params differ diff --git a/common/tvm_utility/data/models/abs_model_aarch64/inference_engine_tvm_config.hpp b/common/tvm_utility/data/models/abs_model_aarch64/inference_engine_tvm_config.hpp new file mode 100644 index 0000000000000..09c8c0beacebe --- /dev/null +++ b/common/tvm_utility/data/models/abs_model_aarch64/inference_engine_tvm_config.hpp @@ -0,0 +1,54 @@ +// Copyright 2021 Arm Limited and 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 "tvm_utility/pipeline.hpp" + +#ifndef COMMON__TVM_UTILITY__DATA__MODELS__ABS_MODEL_AARCH64__INFERENCE_ENGINE_TVM_CONFIG_HPP_ // NOLINT +#define COMMON__TVM_UTILITY__DATA__MODELS__ABS_MODEL_AARCH64__INFERENCE_ENGINE_TVM_CONFIG_HPP_ + +namespace model_zoo +{ +namespace inf_test +{ +namespace engine_load +{ +namespace abs_model +{ + +static const tvm_utility::pipeline::InferenceEngineTVMConfig config{ + {0, 0, 0}, // modelzoo_version + + // cspell: ignore mtriple + "abs_model_aarch64", // network_name + "llvm -mtriple=aarch64-linux-gnu", // network_backend + + "deploy_lib.so", // network_module_path + "deploy_graph.json", // network_graph_path + "deploy_param.params", // network_params_path + + // cspell: ignore DLCPU + kDLCPU, // tvm_device_type + 0, // tvm_device_id + + {{"a", kDLFloat, 32, 1, {2, 2}}}, // network_inputs + + {{"output", kDLFloat, 32, 1, {2, 2}}} // network_outputs +}; + +} // namespace abs_model +} // namespace engine_load +} // namespace inf_test +} // namespace model_zoo +#endif // COMMON__TVM_UTILITY__DATA__MODELS__ABS_MODEL_AARCH64__INFERENCE_ENGINE_TVM_CONFIG_HPP_ + // NOLINT diff --git a/common/tvm_utility/data/models/abs_model_x86_64/deploy_graph.json b/common/tvm_utility/data/models/abs_model_x86_64/deploy_graph.json new file mode 100644 index 0000000000000..b226c01747dca --- /dev/null +++ b/common/tvm_utility/data/models/abs_model_x86_64/deploy_graph.json @@ -0,0 +1,36 @@ +{ + "nodes": [ + { + "op": "null", + "name": "a", + "inputs": [] + }, + { + "op": "tvm_op", + "name": "tvmgen_default_fused_abs", + "attrs": { + "num_outputs": "1", + "num_inputs": "1", + "flatten_data": "0", + "func_name": "tvmgen_default_fused_abs", + "hash": "1be44995aa501758" + }, + "inputs": [[0, 0, 0]] + } + ], + "arg_nodes": [0], + "heads": [[1, 0, 0]], + "attrs": { + "dltype": ["list_str", ["float32", "float32"]], + "device_index": ["list_int", [1, 1]], + "storage_id": ["list_int", [0, 1]], + "shape": [ + "list_shape", + [ + [2, 2], + [2, 2] + ] + ] + }, + "node_row_ptr": [0, 1, 2] +} diff --git a/common/tvm_utility/data/models/abs_model_x86_64/deploy_lib.so b/common/tvm_utility/data/models/abs_model_x86_64/deploy_lib.so new file mode 100644 index 0000000000000..9a6d02817e048 Binary files /dev/null and b/common/tvm_utility/data/models/abs_model_x86_64/deploy_lib.so differ diff --git a/common/tvm_utility/data/models/abs_model_x86_64/deploy_param.params b/common/tvm_utility/data/models/abs_model_x86_64/deploy_param.params new file mode 100644 index 0000000000000..1011def01ed82 Binary files /dev/null and b/common/tvm_utility/data/models/abs_model_x86_64/deploy_param.params differ diff --git a/common/tvm_utility/data/models/abs_model_x86_64/inference_engine_tvm_config.hpp b/common/tvm_utility/data/models/abs_model_x86_64/inference_engine_tvm_config.hpp new file mode 100644 index 0000000000000..7a7e3ef97c1b3 --- /dev/null +++ b/common/tvm_utility/data/models/abs_model_x86_64/inference_engine_tvm_config.hpp @@ -0,0 +1,53 @@ +// Copyright 2021 Arm Limited and 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 "tvm_utility/pipeline.hpp" + +#ifndef COMMON__TVM_UTILITY__DATA__MODELS__ABS_MODEL_X86_64__INFERENCE_ENGINE_TVM_CONFIG_HPP_ // NOLINT +#define COMMON__TVM_UTILITY__DATA__MODELS__ABS_MODEL_X86_64__INFERENCE_ENGINE_TVM_CONFIG_HPP_ + +namespace model_zoo +{ +namespace inf_test +{ +namespace engine_load +{ +namespace abs_model +{ + +static const tvm_utility::pipeline::InferenceEngineTVMConfig config{ + {0, 0, 0}, // modelzoo_version + + "abs_model_x86_64", // network_name + "llvm", // network_backend + + "deploy_lib.so", // network_module_path + "deploy_graph.json", // network_graph_path + "deploy_param.params", // network_params_path + + // cspell: ignore DLCPU + kDLCPU, // tvm_device_type + 0, // tvm_device_id + + {{"a", kDLFloat, 32, 1, {2, 2}}}, // network_inputs + + {{"output", kDLFloat, 32, 1, {2, 2}}} // network_outputs +}; + +} // namespace abs_model +} // namespace engine_load +} // namespace inf_test +} // namespace model_zoo +#endif // COMMON__TVM_UTILITY__DATA__MODELS__ABS_MODEL_X86_64__INFERENCE_ENGINE_TVM_CONFIG_HPP_ + // NOLINT diff --git a/common/tvm_utility/data/models/yolo_v2_tiny/inference_engine_tvm_config.hpp b/common/tvm_utility/data/models/yolo_v2_tiny/inference_engine_tvm_config.hpp new file mode 100644 index 0000000000000..45ff0d8ce33e3 --- /dev/null +++ b/common/tvm_utility/data/models/yolo_v2_tiny/inference_engine_tvm_config.hpp @@ -0,0 +1,56 @@ +// Copyright 2021 Arm Limited and 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 "tvm_utility/pipeline.hpp" + +#ifndef COMMON__TVM_UTILITY__DATA__MODELS__YOLO_V2_TINY__INFERENCE_ENGINE_TVM_CONFIG_HPP_ // NOLINT +#define COMMON__TVM_UTILITY__DATA__MODELS__YOLO_V2_TINY__INFERENCE_ENGINE_TVM_CONFIG_HPP_ + +namespace model_zoo +{ +namespace perception +{ +namespace camera_obstacle_detection +{ +namespace yolo_v2_tiny +{ +namespace tensorflow_fp32_coco +{ + +static const tvm_utility::pipeline::InferenceEngineTVMConfig config{ + {3, 0, 0}, // modelzoo_version + + "yolo_v2_tiny", // network_name + "llvm", // network_backend + + // cspell: ignore DLCPU + "./deploy_lib.so", // network_module_path + "./deploy_graph.json", // network_graph_path + "./deploy_param.params", // network_params_path + + kDLCPU, // tvm_device_type + 0, // tvm_device_id + + {{"input", kDLFloat, 32, 1, {-1, 416, 416, 3}}}, // network_inputs + + {{"output", kDLFloat, 32, 1, {1, 13, 13, 425}}} // network_outputs +}; + +} // namespace tensorflow_fp32_coco +} // namespace yolo_v2_tiny +} // namespace camera_obstacle_detection +} // namespace perception +} // namespace model_zoo +#endif // COMMON__TVM_UTILITY__DATA__MODELS__YOLO_V2_TINY__INFERENCE_ENGINE_TVM_CONFIG_HPP_ + // NOLINT diff --git a/common/tvm_utility/test/yolo_v2_tiny/main.cpp b/common/tvm_utility/example/yolo_v2_tiny/main.cpp similarity index 80% rename from common/tvm_utility/test/yolo_v2_tiny/main.cpp rename to common/tvm_utility/example/yolo_v2_tiny/main.cpp index aac7900f423c2..8a89436170894 100644 --- a/common/tvm_utility/test/yolo_v2_tiny/main.cpp +++ b/common/tvm_utility/example/yolo_v2_tiny/main.cpp @@ -12,30 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "gtest/gtest.h" #include "tvm_utility/pipeline.hpp" #include "yolo_v2_tiny/inference_engine_tvm_config.hpp" #include +#include #include +#include +#include #include #include #include using model_zoo::perception::camera_obstacle_detection::yolo_v2_tiny::tensorflow_fp32_coco::config; -// Name of file containing the human readable names of the classes. One class -// on each line. -static constexpr const char * LABEL_FILENAME = "./yolo_v2_tiny_artifacts/labels.txt"; - -// Name of file containing the anchor values for the network. Each line is one -// anchor. each anchor has 2 comma separated floating point values. -static constexpr const char * ANCHOR_FILENAME = "./yolo_v2_tiny_artifacts/anchors.csv"; - -// Filename of the image on which to run the inference -static constexpr const char * IMAGE_FILENAME = "./yolo_v2_tiny_artifacts/test_image_0.jpg"; - namespace tvm_utility { namespace yolo_v2_tiny @@ -118,16 +109,18 @@ class PreProcessorYoloV2Tiny : public tvm_utility::pipeline::PreProcessor> { public: - explicit PostProcessorYoloV2Tiny(tvm_utility::pipeline::InferenceEngineTVMConfig config) + explicit PostProcessorYoloV2Tiny( + tvm_utility::pipeline::InferenceEngineTVMConfig config, std::string label_filename, + std::string anchor_filename) : network_output_width(config.network_outputs[0].node_shape[1]), network_output_height(config.network_outputs[0].node_shape[2]), network_output_depth(config.network_outputs[0].node_shape[3]), network_output_datatype_bytes(config.network_outputs[0].tvm_dtype_bits / 8) { // Parse human readable names for the classes - std::ifstream label_file{LABEL_FILENAME}; + std::ifstream label_file{label_filename}; if (!label_file.good()) { - std::string label_filename = LABEL_FILENAME; + std::string label_filename = label_filename; throw std::runtime_error("unable to open label file:" + label_filename); } std::string line{}; @@ -136,9 +129,9 @@ class PostProcessorYoloV2Tiny : public tvm_utility::pipeline::PostProcessor> anchors{}; }; -TEST(PipelineExamples, SimplePipeline) +} // namespace yolo_v2_tiny +} // namespace tvm_utility + +bool check_near(double expected, double actual, double tolerance) +{ + return fabs(expected - actual) <= tolerance; +} + +int main(int argc, char * argv[]) { + // init node to use parameters + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("yolo_v2_tiny_example"); + // Filename of the image on which to run the inference + node->declare_parameter("image_filename"); + // Name of file containing the human readable names of the classes. One class + // on each line. + node->declare_parameter("label_filename"); + // Name of file containing the anchor values for the network. Each line is one + // anchor. each anchor has 2 comma separated floating point values. + node->declare_parameter("anchor_filename"); + // Packages data and artifacts directory path. + node->declare_parameter("data_path"); + + RCLCPP_INFO(node->get_logger(), "Node started"); + // Instantiate the pipeline - using PrePT = PreProcessorYoloV2Tiny; + using PrePT = tvm_utility::yolo_v2_tiny::PreProcessorYoloV2Tiny; using IET = tvm_utility::pipeline::InferenceEngineTVM; - using PostPT = PostProcessorYoloV2Tiny; + using PostPT = tvm_utility::yolo_v2_tiny::PostProcessorYoloV2Tiny; PrePT PreP{config}; - IET IE{config, "tvm_utility"}; - PostPT PostP{config}; + IET IE{config, "tvm_utility", node->get_parameter("data_path").as_string()}; + PostPT PostP{ + config, node->get_parameter("label_filename").as_string(), + node->get_parameter("anchor_filename").as_string()}; tvm_utility::pipeline::Pipeline pipeline(PreP, IE, PostP); - auto version_status = IE.version_check({2, 0, 0}); - EXPECT_NE(version_status, tvm_utility::Version::Unsupported); - // Push data input the pipeline and get the output - auto output = pipeline.schedule(IMAGE_FILENAME); + auto output = pipeline.schedule(node->get_parameter("image_filename").as_string()); // Define reference vector containing expected values, expressed as hexadecimal integers std::vector int_output{0x3eb64594, 0x3f435656, 0x3ece1600, 0x3e99d381, @@ -271,11 +287,21 @@ TEST(PipelineExamples, SimplePipeline) } // Test: check if the generated output is equal to the reference - EXPECT_EQ(expected_output.size(), output.size()) << "Unexpected output size"; + if (expected_output.size() == output.size()) { + RCLCPP_INFO(node->get_logger(), "Model has proper output size"); + } else { + RCLCPP_INFO(node->get_logger(), "Model has unexpected output size"); + } + for (size_t i = 0; i < output.size(); ++i) { - EXPECT_NEAR(expected_output[i], output[i], 0.0001) << "at index: " << i; + if (check_near(expected_output[i], output[i], 0.0001)) { + std::cout << "Model has proper output at index: " << i << std::endl; + RCLCPP_INFO(node->get_logger(), "Model has proper output at index: %zu", i); + + } else { + RCLCPP_INFO(node->get_logger(), "Model has unexpected output at index: %zu", i); + } } + rclcpp::shutdown(); + return 0; } - -} // namespace yolo_v2_tiny -} // namespace tvm_utility diff --git a/common/tvm_utility/include/tvm_utility/pipeline.hpp b/common/tvm_utility/include/tvm_utility/pipeline.hpp index 8504da193214f..a053d5ee471be 100644 --- a/common/tvm_utility/include/tvm_utility/pipeline.hpp +++ b/common/tvm_utility/include/tvm_utility/pipeline.hpp @@ -224,12 +224,19 @@ typedef struct class InferenceEngineTVM : public InferenceEngine { public: - explicit InferenceEngineTVM(const InferenceEngineTVMConfig & config, const std::string & pkg_name) + explicit InferenceEngineTVM( + const InferenceEngineTVMConfig & config, const std::string & pkg_name, + const std::string & autoware_data_path = "") : config_(config) { // Get full network path - std::string network_prefix = ament_index_cpp::get_package_share_directory(pkg_name) + - "/models/" + config.network_name + "/"; + std::string network_prefix; + if (autoware_data_path == "") { + network_prefix = ament_index_cpp::get_package_share_directory(pkg_name) + "/models/" + + config.network_name + "/"; + } else { + network_prefix = autoware_data_path + "/" + pkg_name + "/models/" + config.network_name + "/"; + } std::string network_module_path = network_prefix + config.network_module_path; std::string network_graph_path = network_prefix + config.network_graph_path; std::string network_params_path = network_prefix + config.network_params_path; diff --git a/common/tvm_utility/launch/yolo_v2_tiny_example.launch.xml b/common/tvm_utility/launch/yolo_v2_tiny_example.launch.xml new file mode 100644 index 0000000000000..045a6fc9dfa27 --- /dev/null +++ b/common/tvm_utility/launch/yolo_v2_tiny_example.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + diff --git a/common/tvm_utility/schema/yolo_v2_tiny_example.schema.json b/common/tvm_utility/schema/yolo_v2_tiny_example.schema.json new file mode 100644 index 0000000000000..8ee1987f73a62 --- /dev/null +++ b/common/tvm_utility/schema/yolo_v2_tiny_example.schema.json @@ -0,0 +1,47 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for yolo_v2_tiny_example of tvm_utility", + "type": "object", + "definitions": { + "yolo_v2_tiny_example": { + "type": "object", + "properties": { + "image_filename": { + "type": "string", + "default": "$(find-pkg-share tvm_utility)/artifacts/yolo_v2_tiny/test_image_0.jpg", + "description": "Filename of the image on which to run the inference." + }, + "label_filename": { + "type": "string", + "default": "$(find-pkg-share tvm_utility)/artifacts/yolo_v2_tiny/labels.txt", + "description": "Name of file containing the human readable names of the classes. One class on each line." + }, + "anchor_filename": { + "type": "string", + "default": "$(find-pkg-share tvm_utility)/artifacts/yolo_v2_tiny/anchors.csv", + "description": "Name of file containing the anchor values for the network. Each line is one anchor. each anchor has 2 comma separated floating point values." + }, + "data_path": { + "type": "string", + "default": "$(env HOME)/autoware_data", + "description": "Packages data and artifacts directory path." + } + }, + "required": ["image_filename", "label_filename", "anchor_filename", "data_path"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/yolo_v2_tiny_example" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/common/tvm_utility/test/abs_model/main.cpp b/common/tvm_utility/test/abs_model/main.cpp new file mode 100644 index 0000000000000..4bf1a69c0556b --- /dev/null +++ b/common/tvm_utility/test/abs_model/main.cpp @@ -0,0 +1,146 @@ +// Copyright 2021-2022 Arm Limited and 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 "gtest/gtest.h" +#include "tvm_utility/pipeline.hpp" +// file for current arch x86 or arm is chosen in cmake file +#include +#include + +#include +#include +#include +#include +#include + +using model_zoo::inf_test::engine_load::abs_model::config; + +namespace tvm_utility +{ +namespace abs_model +{ + +class PreProcessorLinearModel : public tvm_utility::pipeline::PreProcessor> +{ +public: + explicit PreProcessorLinearModel(tvm_utility::pipeline::InferenceEngineTVMConfig config) + : network_input_a_width(config.network_inputs[0].node_shape[0]), + network_input_a_height(config.network_inputs[0].node_shape[1]), + network_input_datatype_bytes(config.network_inputs[0].tvm_dtype_bits / 8) + { + // Allocate input variable + std::vector shape_a{network_input_a_width, network_input_a_height}; + tvm_utility::pipeline::TVMArrayContainer a{ + shape_a, + config.network_inputs[0].tvm_dtype_code, + config.network_inputs[0].tvm_dtype_bits, + config.network_inputs[0].tvm_dtype_lanes, + config.tvm_device_type, + config.tvm_device_id}; + + output = a; + } + + // The cv::Mat can't be used as an input because it throws an exception when + // passed as a constant reference + tvm_utility::pipeline::TVMArrayContainerVector schedule(const std::vector & input) + { + float input_mat[2][2]; + input_mat[0][0] = input[0]; + input_mat[0][1] = input[1]; + input_mat[1][0] = input[2]; + input_mat[1][1] = input[3]; + + // Create cv::Mat from input array + cv::Mat a_input = cv::Mat(2, 2, CV_32F, &input_mat); + + TVMArrayCopyFromBytes( + output.getArray(), a_input.data, + network_input_a_width * network_input_a_height * network_input_datatype_bytes); + + return {output}; + } + +private: + int64_t network_input_a_width; + int64_t network_input_a_height; + int64_t network_input_datatype_bytes; + tvm_utility::pipeline::TVMArrayContainer output; +}; + +class PostProcessorLinearModel : public tvm_utility::pipeline::PostProcessor> +{ +public: + explicit PostProcessorLinearModel(tvm_utility::pipeline::InferenceEngineTVMConfig config) + : network_output_width(config.network_outputs[0].node_shape[0]), + network_output_height(config.network_outputs[0].node_shape[1]), + network_output_datatype_bytes(config.network_outputs[0].tvm_dtype_bits / 8) + { + } + + std::vector schedule(const tvm_utility::pipeline::TVMArrayContainerVector & input) + { + // Assert data is stored row-majored in input and the dtype is float + assert(input[0].getArray()->strides == nullptr); + assert(input[0].getArray()->dtype.bits == sizeof(float) * 8); + + // Copy the inference data to CPU memory + std::vector infer(network_output_width * network_output_height, 0.0f); + + TVMArrayCopyToBytes( + input[0].getArray(), infer.data(), + network_output_width * network_output_height * network_output_datatype_bytes); + + return infer; + } + +private: + int64_t network_output_width; + int64_t network_output_height; + int64_t network_output_datatype_bytes; +}; + +TEST(PipelineExamples, SimplePipeline) +{ + // // Instantiate the pipeline + using PrePT = PreProcessorLinearModel; + using IET = tvm_utility::pipeline::InferenceEngineTVM; + using PostPT = PostProcessorLinearModel; + + PrePT PreP{config}; + IET IE{config, "tvm_utility"}; + PostPT PostP{config}; + + tvm_utility::pipeline::Pipeline pipeline(PreP, IE, PostP); + + auto version_status = IE.version_check({2, 0, 0}); + EXPECT_NE(version_status, tvm_utility::Version::Unsupported); + + // create input array + std::vector input_arr{-1., -2., -3., 4.}; + // send it to the model + auto output = pipeline.schedule(input_arr); + + // define vector with expected values + std::vector expected_output{1., 2., 3., 4.}; + + // // Test: check if the generated output is equal to the reference + EXPECT_EQ(expected_output.size(), output.size()) << "Unexpected output size"; + for (size_t i = 0; i < output.size(); ++i) { + EXPECT_NEAR(expected_output[i], output[i], 0.0001) << "at index: " << i; + } +} + +} // namespace abs_model +} // namespace tvm_utility diff --git a/common/tvm_utility/tvm-utility-yolo-v2-tiny-tests.md b/common/tvm_utility/tvm-utility-yolo-v2-tiny-tests.md index 188918a8f74df..39bcc640c2147 100644 --- a/common/tvm_utility/tvm-utility-yolo-v2-tiny-tests.md +++ b/common/tvm_utility/tvm-utility-yolo-v2-tiny-tests.md @@ -7,20 +7,41 @@ output. ## Compiling the Example -1. Download an example image to be used as test input. this image needs to be - saved in the `artifacts/yolo_v2_tiny/` folder + -```sh -curl https://raw.githubusercontent.com/pjreddie/darknet/master/data/dog.jpg \ - > artifacts/yolo_v2_tiny/test_image_0.jpg -``` +1. Check if model was downloaded during the env preparation step by ansible and + models files exist in the folder $HOME/autoware_data/tvm_utility/models/yolo_v2_tiny. -1. Build and test with the `DOWNLOAD_ARTIFACTS` flag set. + If not you can download them manually, see [Manual Artifacts Downloading](https://github.com/autowarefoundation/autoware/tree/main/ansible/roles/artifacts). -```sh -colcon build --packages-up-to tvm_utility --cmake-args -DDOWNLOAD_ARTIFACTS=ON -colcon test --packages-select tvm_utility -``` +2. Download an example image to be used as test input. This image needs to be + saved in the `artifacts/yolo_v2_tiny/` folder. + + ```sh + curl https://raw.githubusercontent.com/pjreddie/darknet/master/data/dog.jpg \ + > artifacts/yolo_v2_tiny/test_image_0.jpg + ``` + +3. Build. + + ```sh + colcon build --packages-up-to tvm_utility --cmake-args -DBUILD_EXAMPLE=ON + ``` + +4. Run. + + ```sh + ros2 launch tvm_utility yolo_v2_tiny_example.launch.xml + ``` + +## Parameters + +| Name | Type | Default Value | Description | +| ----------------- | ------ | ----------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------- | +| `image_filename` | string | `$(find-pkg-share tvm_utility)/artifacts/yolo_v2_tiny/test_image_0.jpg` | Filename of the image on which to run the inference. | +| `label_filename` | string | `$(find-pkg-share tvm_utility)/artifacts/yolo_v2_tiny/labels.txt` | Name of file containing the human readable names of the classes. One class on each line. | +| `anchor_filename` | string | `$(find-pkg-share tvm_utility)/artifacts/yolo_v2_tiny/anchors.csv` | Name of file containing the anchor values for the network. Each line is one anchor. each anchor has 2 comma separated floating point values. | +| `data_path` | string | `$(env HOME)/autoware_data` | Packages data and artifacts directory path. | ## GPU backend @@ -28,5 +49,5 @@ Vulkan is supported by default by the tvm_vendor package. It can be selected by setting the `tvm_utility_BACKEND` variable: ```sh -colcon build --packages-up-to tvm_utility --cmake-args -DDOWNLOAD_ARTIFACTS=ON -Dtvm_utility_BACKEND=vulkan +colcon build --packages-up-to tvm_utility -Dtvm_utility_BACKEND=vulkan ``` diff --git a/common/tvm_utility/tvm_utility-extras.cmake b/common/tvm_utility/tvm_utility-extras.cmake index 4214aa4995f0f..414644c1fe041 100644 --- a/common/tvm_utility/tvm_utility-extras.cmake +++ b/common/tvm_utility/tvm_utility-extras.cmake @@ -12,12 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -# Get user-provided variables -set(DOWNLOAD_ARTIFACTS OFF CACHE BOOL "enable artifacts download") -set(MODELZOO_VERSION "3.0.0-20221221" CACHE STRING "targeted ModelZoo version") - # -# Download the selected neural network if it is not already present on disk. # Make inference_engine_tvm_config.hpp available under "data/models/${MODEL_NAME}/". # Install the TVM artifacts to "share/${PROJECT_NAME}/models/". # Return the name of the custom target in the DEPENDENCY parameter. @@ -34,48 +29,16 @@ function(get_neural_network MODEL_NAME MODEL_BACKEND DEPENDENCY) set(EXTERNALPROJECT_NAME ${MODEL_NAME}_${MODEL_BACKEND}) set(PREPROCESSING "") - # Prioritize user-provided models. - # cspell: ignore COPYONLY - if(IS_DIRECTORY "${DATA_PATH}/user/${MODEL_NAME}") - message(STATUS "Using user-provided model from ${DATA_PATH}/user/${MODEL_NAME}") - file(REMOVE_RECURSE "${DATA_PATH}/models/${MODEL_NAME}/") - configure_file( - "${DATA_PATH}/user/${MODEL_NAME}/inference_engine_tvm_config.hpp" - "${DATA_PATH}/models/${MODEL_NAME}/inference_engine_tvm_config.hpp" - COPYONLY - ) - if(EXISTS "${DATA_PATH}/user/${MODEL_NAME}/preprocessing_inference_engine_tvm_config.hpp") - configure_file( - "${DATA_PATH}/user/${MODEL_NAME}/preprocessing_inference_engine_tvm_config.hpp" - "${DATA_PATH}/models/${MODEL_NAME}/preprocessing_inference_engine_tvm_config.hpp" - COPYONLY - ) - endif() - set(SOURCE_DIR "${DATA_PATH}/user/${MODEL_NAME}") - set(INSTALL_DIRECTORY "${DATA_PATH}/user/${MODEL_NAME}") - else() - set(ARCHIVE_NAME "${MODEL_NAME}-${CMAKE_SYSTEM_PROCESSOR}-${MODEL_BACKEND}-${MODELZOO_VERSION}.tar.gz") - - # Use previously-downloaded archives if available. - set(DOWNLOAD_DIR "${DATA_PATH}/downloads") - if(DOWNLOAD_ARTIFACTS) - message(STATUS "Downloading ${ARCHIVE_NAME} ...") - if(NOT EXISTS "${DATA_PATH}/downloads/${ARCHIVE_NAME}") - set(URL "https://autoware-modelzoo.s3.us-east-2.amazonaws.com/models/${MODELZOO_VERSION}/${ARCHIVE_NAME}") - file(DOWNLOAD ${URL} "${DOWNLOAD_DIR}/${ARCHIVE_NAME}") - endif() - else() - message(WARNING "Skipped download for ${MODEL_NAME} (enable by setting DOWNLOAD_ARTIFACTS)") - set(${DEPENDENCY} "" PARENT_SCOPE) - return() - endif() + if(IS_DIRECTORY "${DATA_PATH}/models/${MODEL_NAME}") set(SOURCE_DIR "${DATA_PATH}/models/${MODEL_NAME}") set(INSTALL_DIRECTORY "${DATA_PATH}/models/${MODEL_NAME}") - file(ARCHIVE_EXTRACT INPUT "${DOWNLOAD_DIR}/${ARCHIVE_NAME}" DESTINATION "${SOURCE_DIR}") if(EXISTS "${DATA_PATH}/models/${MODEL_NAME}/preprocessing_inference_engine_tvm_config.hpp") set(PREPROCESSING "${DATA_PATH}/models/${MODEL_NAME}/preprocessing_inference_engine_tvm_config.hpp") endif() - + else() + message(WARNING "No model configuration files were provided") + set(${DEPENDENCY} "" PARENT_SCOPE) + return() endif() include(ExternalProject) diff --git a/control/autonomous_emergency_braking/README.md b/control/autonomous_emergency_braking/README.md index d1cd80625f2ac..4eff1583d5df8 100644 --- a/control/autonomous_emergency_braking/README.md +++ b/control/autonomous_emergency_braking/README.md @@ -26,22 +26,25 @@ This module has following assumptions. ## Parameters -| Name | Unit | Type | Description | Default value | -| :------------------------ | :----- | :----- | :-------------------------------------------------------------------------- | :------------ | -| use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | -| use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | -| voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 | -| voxel_grid_y | [m] | double | down sampling parameters of y-axis for voxel grid filter | 0.05 | -| voxel_grid_z | [m] | double | down sampling parameters of z-axis for voxel grid filter | 100000.0 | -| min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | -| expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 | -| longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 | -| t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 | -| a_ego_min | [m/ss] | double | maximum deceleration value of the ego vehicle | -3.0 | -| a_obj_min | [m/ss] | double | maximum deceleration value of objects | -3.0 | -| prediction_time_horizon | [s] | double | time horizon of the predicted path generated by sensors | 1.5 | -| prediction_time_interval | [s] | double | time interval of the predicted path generated by sensors | 0.1 | -| aeb_hz | [-] | double | 　frequency at which AEB operates per second | 10 | +| Name | Unit | Type | Description | Default value | +| :--------------------------- | :----- | :----- | :-------------------------------------------------------------------------- | :------------ | +| publish_debug_pointcloud | [-] | bool | flag to publish the point cloud used for debugging | false | +| use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | +| use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | +| voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 | +| voxel_grid_y | [m] | double | down sampling parameters of y-axis for voxel grid filter | 0.05 | +| voxel_grid_z | [m] | double | down sampling parameters of z-axis for voxel grid filter | 100000.0 | +| min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | +| expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 | +| longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 | +| t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 | +| a_ego_min | [m/ss] | double | maximum deceleration value of the ego vehicle | -3.0 | +| a_obj_min | [m/ss] | double | maximum deceleration value of objects | -3.0 | +| imu_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by sensors | 1.5 | +| imu_prediction_time_interval | [s] | double | time interval of the predicted path generated by sensors | 0.1 | +| mpc_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by mpc | 1.5 | +| mpc_prediction_time_interval | [s] | double | time interval of the predicted path generated by mpc | 0.1 | +| aeb_hz | [-] | double | frequency at which AEB operates per second | 10 | ## Inner-workings / Algorithms diff --git a/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 09db0feb34597..1a870aff7a843 100644 --- a/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + publish_debug_pointcloud: false use_predicted_trajectory: true use_imu_path: true voxel_grid_x: 0.05 @@ -11,7 +12,9 @@ t_response: 1.0 a_ego_min: -3.0 a_obj_min: -1.0 - prediction_time_horizon: 1.5 - prediction_time_interval: 0.1 + imu_prediction_time_horizon: 1.5 + imu_prediction_time_interval: 0.1 + mpc_prediction_time_horizon: 1.5 + mpc_prediction_time_interval: 0.1 collision_keeping_sec: 0.0 aeb_hz: 10.0 diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp index a8f260795d485..27ec775110109 100644 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -172,6 +172,7 @@ class AEB : public rclcpp::Node Updater updater_{this}; // member variables + bool publish_debug_pointcloud_; bool use_predicted_trajectory_; bool use_imu_path_; double voxel_grid_x_; @@ -183,8 +184,10 @@ class AEB : public rclcpp::Node double t_response_; double a_ego_min_; double a_obj_min_; - double prediction_time_horizon_; - double prediction_time_interval_; + double imu_prediction_time_horizon_; + double imu_prediction_time_interval_; + double mpc_prediction_time_horizon_; + double mpc_prediction_time_interval_; CollisionDataKeeper collision_data_keeper_; }; } // namespace autoware::motion::control::autonomous_emergency_braking diff --git a/control/autonomous_emergency_braking/package.xml b/control/autonomous_emergency_braking/package.xml index 5dc65cb243bfc..d45078064da3c 100644 --- a/control/autonomous_emergency_braking/package.xml +++ b/control/autonomous_emergency_braking/package.xml @@ -4,7 +4,6 @@ autonomous_emergency_braking 0.1.0 Autonomous Emergency Braking package as a ROS 2 node - Yutaka Shimizu Takamasa Horibe Tomoya Kimura diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index 0ccc9bc4a3dae..e88a5ee833612 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -125,6 +125,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) updater_.add("aeb_emergency_stop", this, &AEB::onCheckCollision); // parameter + publish_debug_pointcloud_ = declare_parameter("publish_debug_pointcloud"); use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); use_imu_path_ = declare_parameter("use_imu_path"); voxel_grid_x_ = declare_parameter("voxel_grid_x"); @@ -136,8 +137,10 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) t_response_ = declare_parameter("t_response"); a_ego_min_ = declare_parameter("a_ego_min"); a_obj_min_ = declare_parameter("a_obj_min"); - prediction_time_horizon_ = declare_parameter("prediction_time_horizon"); - prediction_time_interval_ = declare_parameter("prediction_time_interval"); + imu_prediction_time_horizon_ = declare_parameter("imu_prediction_time_horizon"); + imu_prediction_time_interval_ = declare_parameter("imu_prediction_time_interval"); + mpc_prediction_time_horizon_ = declare_parameter("mpc_prediction_time_horizon"); + mpc_prediction_time_interval_ = declare_parameter("mpc_prediction_time_interval"); const auto collision_keeping_sec = declare_parameter("collision_keeping_sec"); collision_data_keeper_.setTimeout(collision_keeping_sec); @@ -227,7 +230,9 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) obstacle_ros_pointcloud_ptr_ = std::make_shared(); pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_); obstacle_ros_pointcloud_ptr_->header = input_msg->header; - pub_obstacle_pointcloud_->publish(*obstacle_ros_pointcloud_ptr_); + if (publish_debug_pointcloud_) { + pub_obstacle_pointcloud_->publish(*obstacle_ros_pointcloud_ptr_); + } } bool AEB::isDataReady() @@ -392,8 +397,8 @@ void AEB::generateEgoPath( } constexpr double epsilon = 1e-6; - const double & dt = prediction_time_interval_; - const double & horizon = prediction_time_horizon_; + const double & dt = imu_prediction_time_interval_; + const double & horizon = imu_prediction_time_horizon_; for (double t = 0.0; t < horizon + epsilon; t += dt) { curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; @@ -452,8 +457,11 @@ void AEB::generateEgoPath( geometry_msgs::msg::Pose map_pose; tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped); path.at(i) = map_pose; - } + if (i * mpc_prediction_time_interval_ > mpc_prediction_time_horizon_) { + break; + } + } // create polygon polygons.resize(path.size()); for (size_t i = 0; i < path.size() - 1; ++i) { diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp index e7aa644ad63f9..ca30bd30e4dd1 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp @@ -45,6 +45,10 @@ class QPSolverInterface const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a, const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a, const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) = 0; + + virtual int64_t getTakenIter() const { return 0; } + virtual double getRunTime() const { return 0.0; } + virtual double getObjVal() const { return 0.0; } }; } // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp index cdb590faab84d..2611f94da324f 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp @@ -53,6 +53,10 @@ class QPSolverOSQP : public QPSolverInterface const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a, const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) override; + int64_t getTakenIter() const override { return osqpsolver_.getTakenIter(); } + double getRunTime() const override { return osqpsolver_.getRunTime(); } + double getObjVal() const override { return osqpsolver_.getObjVal(); } + private: autoware::common::osqp::OSQPInterface osqpsolver_; rclcpp::Logger logger_; diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp index 3a6bd2b25832b..ef337eaaa8528 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp @@ -53,6 +53,13 @@ class QPSolverEigenLeastSquareLLT : public QPSolverInterface const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a, const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a, const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) override; + + int64_t getTakenIter() const override { return 1; }; + // TODO(someone): calculate runtime and object value, otherwise just return 0 by base class + // double getRunTime() const override { return 0.0; } + // double getObjVal() const override { return 0.0; } + +private: }; } // namespace autoware::motion::control::mpc_lateral_controller #endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 0d7aa8aa8b2e6..6bd885b78beb7 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -147,6 +147,9 @@ Float32MultiArrayStamped MPC::generateDiagData( const double wz_predicted = current_velocity * std::tan(mpc_data.predicted_steer) / wb; const double wz_measured = current_velocity * std::tan(mpc_data.steer) / wb; const double wz_command = current_velocity * std::tan(ctrl_cmd.steering_tire_angle) / wb; + const int iteration_num = m_qpsolver_ptr->getTakenIter(); + const double runtime = m_qpsolver_ptr->getRunTime(); + const double objective_value = m_qpsolver_ptr->getObjVal(); typedef decltype(diagnostic.data)::value_type DiagnosticValueType; const auto append_diag = [&](const auto & val) -> void { @@ -170,6 +173,9 @@ Float32MultiArrayStamped MPC::generateDiagData( append_diag(nearest_k); // [15] nearest path curvature (not smoothed) append_diag(mpc_data.predicted_steer); // [16] predicted steer append_diag(wz_predicted); // [17] angular velocity from predicted steer + append_diag(iteration_num); // [18] iteration number + append_diag(runtime); // [19] runtime of the latest problem solved + append_diag(objective_value); // [20] objective value of the latest problem solved return diagnostic; } diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index a134fd775155b..d13e628f2e1d4 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -497,8 +497,13 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d stop_dist > p.drive_state_stop_dist + p.drive_state_offset_stop_dist; const bool departure_condition_from_stopped = stop_dist > p.drive_state_stop_dist; - const bool keep_stopped_condition = - m_enable_keep_stopped_until_steer_convergence && !lateral_sync_data_.is_steer_converged; + // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity + static constexpr double vel_epsilon = 1e-3; + + // Let vehicle start after the steering is converged for control accuracy + const bool keep_stopped_condition = std::fabs(current_vel) < vel_epsilon && + m_enable_keep_stopped_until_steer_convergence && + !lateral_sync_data_.is_steer_converged; const bool stopping_condition = stop_dist < p.stopping_state_stop_dist; @@ -528,8 +533,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d ? (clock_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time : false; - static constexpr double vel_epsilon = - 1e-3; // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity const double current_vel_cmd = std::fabs(m_trajectory.points.at(control_data.nearest_idx).longitudinal_velocity_mps); const bool emergency_condition = m_enable_overshoot_emergency && diff --git a/control/vehicle_cmd_gate/README.md b/control/vehicle_cmd_gate/README.md index 231c2c5022138..6ec2da84a7b6c 100644 --- a/control/vehicle_cmd_gate/README.md +++ b/control/vehicle_cmd_gate/README.md @@ -50,6 +50,8 @@ | `check_external_emergency_heartbeat` | bool | true when checking heartbeat for emergency stop | | `system_emergency_heartbeat_timeout` | double | timeout for system emergency | | `external_emergency_stop_heartbeat_timeout` | double | timeout for external emergency | +| `filter_activated_count_threshold` | int | threshold for filter activation | +| `filter_activated_velocity_threshold` | double | velocity threshold for filter activation | | `stop_hold_acceleration` | double | longitudinal acceleration cmd when vehicle should stop | | `emergency_acceleration` | double | longitudinal acceleration cmd when vehicle stop with emergency | | `moderate_stop_service_acceleration` | double | longitudinal acceleration cmd when vehicle stop with moderate stop service | diff --git a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index 191e894622af7..7ad685217f13d 100644 --- a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -6,6 +6,8 @@ check_external_emergency_heartbeat: false use_start_request: false enable_cmd_limit_filter: true + filter_activated_count_threshold: 5 + filter_activated_velocity_threshold: 1.0 external_emergency_stop_heartbeat_timeout: 0.0 stop_hold_acceleration: -1.5 emergency_acceleration: -2.4 diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index e681bc22cd24b..1bed4ee67f412 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -77,6 +77,10 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) create_publisher("~/is_filter_activated", durable_qos); filter_activated_marker_pub_ = create_publisher("~/is_filter_activated/marker", durable_qos); + filter_activated_marker_raw_pub_ = + create_publisher("~/is_filter_activated/marker_raw", durable_qos); + filter_activated_flag_pub_ = + create_publisher("~/is_filter_activated/flag", durable_qos); // Subscriber external_emergency_stop_heartbeat_sub_ = create_subscription( @@ -160,6 +164,9 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) declare_parameter("moderate_stop_service_acceleration"); stop_check_duration_ = declare_parameter("stop_check_duration"); enable_cmd_limit_filter_ = declare_parameter("enable_cmd_limit_filter"); + filter_activated_count_threshold_ = declare_parameter("filter_activated_count_threshold"); + filter_activated_velocity_threshold_ = + declare_parameter("filter_activated_velocity_threshold"); // Vehicle Parameter const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -572,7 +579,7 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont is_filter_activated.stamp = now(); is_filter_activated_pub_->publish(is_filter_activated); - filter_activated_marker_pub_->publish(createMarkerArray(is_filter_activated)); + publishMarkers(is_filter_activated); return out; } @@ -787,6 +794,28 @@ MarkerArray VehicleCmdGate::createMarkerArray(const IsFilterActivated & filter_a return msg; } +void VehicleCmdGate::publishMarkers(const IsFilterActivated & filter_activated) +{ + BoolStamped filter_activated_flag; + if (filter_activated.is_activated) { + filter_activated_count_++; + } else { + filter_activated_count_ = 0; + } + if ( + filter_activated_count_ >= filter_activated_count_threshold_ && + std::fabs(current_kinematics_.twist.twist.linear.x) >= filter_activated_velocity_threshold_ && + current_operation_mode_.mode == OperationModeState::AUTONOMOUS) { + filter_activated_marker_pub_->publish(createMarkerArray(filter_activated)); + filter_activated_flag.data = true; + } else { + filter_activated_flag.data = false; + } + + filter_activated_flag.stamp = now(); + filter_activated_flag_pub_->publish(filter_activated_flag); + filter_activated_marker_raw_pub_->publish(createMarkerArray(filter_activated)); +} } // namespace vehicle_cmd_gate #include diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 8f0a6aa14d08b..c36aab240ae79 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -62,6 +62,7 @@ using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::AccelWithCovarianceStamped; using std_srvs::srv::Trigger; using tier4_control_msgs::msg::GateMode; +using tier4_debug_msgs::msg::BoolStamped; using tier4_external_api_msgs::msg::Emergency; using tier4_external_api_msgs::msg::Heartbeat; using tier4_external_api_msgs::srv::SetEmergency; @@ -106,6 +107,8 @@ class VehicleCmdGate : public rclcpp::Node rclcpp::Publisher::SharedPtr operation_mode_pub_; rclcpp::Publisher::SharedPtr is_filter_activated_pub_; rclcpp::Publisher::SharedPtr filter_activated_marker_pub_; + rclcpp::Publisher::SharedPtr filter_activated_marker_raw_pub_; + rclcpp::Publisher::SharedPtr filter_activated_flag_pub_; // Subscription rclcpp::Subscription::SharedPtr external_emergency_stop_heartbeat_sub_; @@ -123,11 +126,13 @@ class VehicleCmdGate : public rclcpp::Node bool is_engaged_; bool is_system_emergency_ = false; bool is_external_emergency_stop_ = false; + bool is_filtered_marker_published_ = false; double current_steer_ = 0; GateMode current_gate_mode_; MrmState current_mrm_state_; Odometry current_kinematics_; double current_acceleration_ = 0.0; + int filter_activated_count_ = 0; // Heartbeat std::shared_ptr emergency_state_heartbeat_received_time_; @@ -172,6 +177,8 @@ class VehicleCmdGate : public rclcpp::Node double emergency_acceleration_; double moderate_stop_service_acceleration_; bool enable_cmd_limit_filter_; + int filter_activated_count_threshold_; + double filter_activated_velocity_threshold_; // Service rclcpp::Service::SharedPtr srv_engage_; @@ -236,6 +243,7 @@ class VehicleCmdGate : public rclcpp::Node // debug MarkerArray createMarkerArray(const IsFilterActivated & filter_activated); + void publishMarkers(const IsFilterActivated & filter_activated); std::unique_ptr logger_configure_; }; diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml index a767d2d253cff..181f470a00800 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml @@ -4,6 +4,7 @@ + @@ -12,12 +13,5 @@ - - - - - - - diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml index b04124d7e981e..12d84f374f7c7 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml @@ -145,7 +145,8 @@ - + + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/fix2pose.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/geo_pose_converter.launch.xml similarity index 55% rename from launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/fix2pose.launch.xml rename to launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/geo_pose_converter.launch.xml index ac92fce40cd6a..d4f82e72a297b 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/fix2pose.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/geo_pose_converter.launch.xml @@ -1,30 +1,23 @@ - - - + + + - + + - - - - - - - - diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 6b3b3d3bbbe01..9a137032e1277 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -16,12 +16,12 @@ ar_tag_based_localizer automatic_pose_initializer - eagleye_fix2pose + eagleye_geo_pose_converter + eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_rt ekf_localizer gyro_odometer - landmark_tf_caster ndt_scan_matcher pointcloud_preprocessor pose_initializer diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index 6ace3f423d1bc..bf81125286256 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -1,48 +1,10 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -59,14 +21,7 @@ - - - - - - - - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml index 5f17256df6b5c..73e466afc0bda 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml @@ -6,12 +6,10 @@ - - - - - - + + + + @@ -19,12 +17,11 @@ - - - - - - + + + + + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py deleted file mode 100644 index 62d4c5b7188ee..0000000000000 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ /dev/null @@ -1,318 +0,0 @@ -# Copyright 2021-2023 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 launch -from launch.actions import DeclareLaunchArgument -from launch.actions import GroupAction -from launch.actions import IncludeLaunchDescription -from launch.actions import OpaqueFunction -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -from launch.conditions import UnlessCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -from launch.substitutions import PythonExpression -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode -from launch_ros.substitutions import FindPackageShare -import yaml - - -def launch_setup(context, *args, **kwargs): - # vehicle information parameter - vehicle_param_path = LaunchConfiguration("vehicle_param_file").perform(context) - with open(vehicle_param_path, "r") as f: - vehicle_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - # common parameter - with open(LaunchConfiguration("common_param_path").perform(context), "r") as f: - common_param = yaml.safe_load(f)["/**"]["ros__parameters"] - # nearest search parameter - with open(LaunchConfiguration("nearest_search_param_path").perform(context), "r") as f: - nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - # behavior path planner - with open(LaunchConfiguration("side_shift_param_path").perform(context), "r") as f: - side_shift_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("avoidance_param_path").perform(context), "r") as f: - avoidance_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("avoidance_by_lc_param_path").perform(context), "r") as f: - avoidance_by_lc_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("dynamic_avoidance_param_path").perform(context), "r") as f: - dynamic_avoidance_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("lane_change_param_path").perform(context), "r") as f: - lane_change_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("goal_planner_param_path").perform(context), "r") as f: - goal_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("start_planner_param_path").perform(context), "r") as f: - start_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("drivable_area_expansion_param_path").perform(context), "r") as f: - drivable_area_expansion_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("scene_module_manager_param_path").perform(context), "r") as f: - scene_module_manager_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(LaunchConfiguration("behavior_path_planner_param_path").perform(context), "r") as f: - behavior_path_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - glog_component = ComposableNode( - package="glog_component", - plugin="GlogComponent", - name="glog_component", - ) - - behavior_path_planner_component = ComposableNode( - package="behavior_path_planner", - plugin="behavior_path_planner::BehaviorPathPlannerNode", - name="behavior_path_planner", - namespace="", - remappings=[ - ("~/input/route", LaunchConfiguration("input_route_topic_name")), - ("~/input/vector_map", LaunchConfiguration("map_topic_name")), - ("~/input/perception", "/perception/object_recognition/objects"), - ("~/input/occupancy_grid_map", "/perception/occupancy_grid_map/map"), - ( - "~/input/costmap", - "/planning/scenario_planning/parking/costmap_generator/occupancy_grid", - ), - ("~/input/odometry", "/localization/kinematic_state"), - ("~/input/accel", "/localization/acceleration"), - ("~/input/scenario", "/planning/scenario_planning/scenario"), - ("~/output/path", "path_with_lane_id"), - ("~/output/turn_indicators_cmd", "/planning/turn_indicators_cmd"), - ("~/output/hazard_lights_cmd", "/planning/hazard_lights_cmd"), - ("~/output/modified_goal", "/planning/scenario_planning/modified_goal"), - ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"), - ], - parameters=[ - common_param, - nearest_search_param, - side_shift_param, - avoidance_param, - avoidance_by_lc_param, - dynamic_avoidance_param, - lane_change_param, - goal_planner_param, - start_planner_param, - drivable_area_expansion_param, - scene_module_manager_param, - behavior_path_planner_param, - vehicle_param, - { - "lane_change.enable_collision_check_at_prepare_phase": LaunchConfiguration( - "use_experimental_lane_change_function" - ), - "lane_change.use_predicted_path_outside_lanelet": LaunchConfiguration( - "use_experimental_lane_change_function" - ), - "lane_change.use_all_predicted_path": LaunchConfiguration( - "use_experimental_lane_change_function" - ), - }, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - # smoother param - with open( - LaunchConfiguration("motion_velocity_smoother_param_path").perform(context), "r" - ) as f: - motion_velocity_smoother_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open( - LaunchConfiguration("behavior_velocity_smoother_type_param_path").perform(context), "r" - ) as f: - behavior_velocity_smoother_type_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - # behavior velocity planner - behavior_velocity_planner_common_param_path = LaunchConfiguration( - "behavior_velocity_planner_common_param_path" - ).perform(context) - behavior_velocity_planner_module_param_paths = LaunchConfiguration( - "behavior_velocity_planner_module_param_paths" - ).perform(context) - - behavior_velocity_planner_params_paths = [ - behavior_velocity_planner_common_param_path, - *yaml.safe_load(behavior_velocity_planner_module_param_paths), - ] - - behavior_velocity_planner_params = {} - for path in behavior_velocity_planner_params_paths: - with open(path) as f: - behavior_velocity_planner_params.update(yaml.safe_load(f)["/**"]["ros__parameters"]) - - behavior_velocity_planner_component = ComposableNode( - package="behavior_velocity_planner", - plugin="behavior_velocity_planner::BehaviorVelocityPlannerNode", - name="behavior_velocity_planner", - namespace="", - remappings=[ - ("~/input/path_with_lane_id", "path_with_lane_id"), - ("~/input/vector_map", "/map/vector_map"), - ("~/input/vehicle_odometry", "/localization/kinematic_state"), - ("~/input/accel", "/localization/acceleration"), - ("~/input/dynamic_objects", "/perception/object_recognition/objects"), - ( - "~/input/no_ground_pointcloud", - "/perception/obstacle_segmentation/pointcloud", - ), - ( - "~/input/compare_map_filtered_pointcloud", - "compare_map_filtered/pointcloud", - ), - ( - "~/input/vector_map_inside_area_filtered_pointcloud", - "vector_map_inside_area_filtered/pointcloud", - ), - ( - "~/input/traffic_signals", - "/perception/traffic_light_recognition/traffic_signals", - ), - ( - "~/input/external_velocity_limit_mps", - "/planning/scenario_planning/max_velocity_default", - ), - ("~/input/virtual_traffic_light_states", "/awapi/tmp/virtual_traffic_light_states"), - ( - "~/input/occupancy_grid", - "/perception/occupancy_grid_map/map", - ), - ("~/output/path", "path"), - ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"), - ( - "~/output/infrastructure_commands", - "/planning/scenario_planning/status/infrastructure_commands", - ), - ("~/output/traffic_signal", "debug/traffic_signal"), - ], - parameters=[ - nearest_search_param, - behavior_velocity_planner_params, - vehicle_param, - common_param, - motion_velocity_smoother_param, - behavior_velocity_smoother_type_param, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - container = ComposableNodeContainer( - name="behavior_planning_container", - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=[ - behavior_path_planner_component, - behavior_velocity_planner_component, - glog_component, - ], - output="screen", - ) - - # This condition is true if run_out module is enabled and its detection method is Points - run_out_module = "behavior_velocity_planner::RunOutModulePlugin" - run_out_method = behavior_velocity_planner_params.get("run_out", {}).get("detection_method") - launch_run_out = run_out_module in behavior_velocity_planner_params["launch_modules"] - launch_run_out_with_points_method = launch_run_out and run_out_method == "Points" - - # load compare map for run_out module - load_compare_map = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [ - FindPackageShare("tier4_planning_launch"), - "/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py", - ], - ), - launch_arguments={ - "use_pointcloud_container": LaunchConfiguration("use_pointcloud_container"), - "container_name": LaunchConfiguration("container_name"), - "use_multithread": "true", - }.items(), - # launch compare map only when run_out module is enabled and detection method is Points - condition=IfCondition(PythonExpression(str(launch_run_out_with_points_method))), - ) - - load_vector_map_inside_area_filter = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [ - FindPackageShare("tier4_planning_launch"), - "/launch/scenario_planning/lane_driving/behavior_planning/vector_map_inside_area_filter.launch.py", - ] - ), - launch_arguments={ - "use_pointcloud_container": LaunchConfiguration("use_pointcloud_container"), - "container_name": LaunchConfiguration("container_name"), - "use_multithread": "true", - "polygon_type": "no_obstacle_segmentation_area_for_run_out", - }.items(), - # launch vector map filter only when run_out module is enabled and detection method is Points - condition=IfCondition(PythonExpression(str(launch_run_out_with_points_method))), - ) - - group = GroupAction( - [ - container, - load_compare_map, - load_vector_map_inside_area_filter, - ] - ) - - return [group] - - -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None, description=None): - launch_arguments.append( - DeclareLaunchArgument(name, default_value=default_value, description=description) - ) - - # vehicle parameter - add_launch_arg("vehicle_param_file") - - # interface parameter - add_launch_arg( - "input_route_topic_name", "/planning/mission_planning/route", "input topic of route" - ) - add_launch_arg("map_topic_name", "/map/vector_map", "input topic of map") - - # package parameter - add_launch_arg("use_experimental_lane_change_function") - - # component - add_launch_arg("use_intra_process", "false", "use ROS 2 component container communication") - add_launch_arg("use_multithread", "false", "use multithread") - - # for points filter of run out module - add_launch_arg("use_pointcloud_container", "true") - add_launch_arg("container_name", "pointcloud_container") - - 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 launch.LaunchDescription( - launch_arguments - + [ - set_container_executable, - set_container_mt_executable, - ] - + [OpaqueFunction(function=launch_setup)] - ) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 4ff862c7852c6..085dc92982663 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -1,30 +1,147 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py deleted file mode 100644 index 474afb4da7d5d..0000000000000 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py +++ /dev/null @@ -1,89 +0,0 @@ -# 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. - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -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 - - -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")), - ) - - composable_nodes = [ - ComposableNode( - package="compare_map_segmentation", - plugin="compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent", - name="voxel_distance_based_compare_map_filter_node", - remappings=[ - ("input", "/perception/obstacle_segmentation/pointcloud"), - ("map", "/map/pointcloud_map"), - ("output", "compare_map_filtered/pointcloud"), - ], - parameters=[ - { - "distance_threshold": 0.7, - } - ], - extra_arguments=[ - {"use_intra_process_comms": False} # this node has QoS of transient local - ], - ), - ] - - compare_map_container = ComposableNodeContainer( - name="compare_map_container", - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=composable_nodes, - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), - output="screen", - ) - - load_composable_nodes = LoadComposableNodes( - composable_node_descriptions=composable_nodes, - target_container=LaunchConfiguration("container_name"), - condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), - ) - - return LaunchDescription( - [ - add_launch_arg("use_multithread", "true"), - add_launch_arg("use_pointcloud_container", "true"), - add_launch_arg("container_name", "compare_map_container"), - set_container_executable, - set_container_mt_executable, - compare_map_container, - load_composable_nodes, - ] - ) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/vector_map_inside_area_filter.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/vector_map_inside_area_filter.launch.py deleted file mode 100644 index 4b3d071dacd7f..0000000000000 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/vector_map_inside_area_filter.launch.py +++ /dev/null @@ -1,88 +0,0 @@ -# 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. - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -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 - - -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")), - ) - - composable_nodes = [ - ComposableNode( - package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VectorMapInsideAreaFilterComponent", - name="vector_map_inside_area_filter_node", - remappings=[ - ("input", "compare_map_filtered/pointcloud"), - ("input/vector_map", "/map/vector_map"), - ("output", "vector_map_inside_area_filtered/pointcloud"), - ], - parameters=[ - { - "polygon_type": LaunchConfiguration("polygon_type"), - } - ], - # this node has QoS of transient local - extra_arguments=[{"use_intra_process_comms": False}], - ), - ] - - vector_map_area_filter_container = ComposableNodeContainer( - name="vector_map_area_filter_container", - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=composable_nodes, - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), - output="screen", - ) - - load_composable_nodes = LoadComposableNodes( - composable_node_descriptions=composable_nodes, - target_container=LaunchConfiguration("container_name"), - condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), - ) - - return LaunchDescription( - [ - add_launch_arg("use_multithread", "true"), - add_launch_arg("use_pointcloud_container", "true"), - add_launch_arg("container_name", "vector_map_area_filter_container"), - set_container_executable, - set_container_mt_executable, - vector_map_area_filter_container, - load_composable_nodes, - ] - ) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py deleted file mode 100644 index d8c5d7825f19d..0000000000000 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ /dev/null @@ -1,387 +0,0 @@ -# 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. - -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import GroupAction -from launch.actions import OpaqueFunction -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -from launch.conditions import LaunchConfigurationEquals -from launch.conditions import UnlessCondition -from launch.substitutions import LaunchConfiguration -from launch.substitutions import PythonExpression -from launch_ros.actions import ComposableNodeContainer -from launch_ros.actions import LoadComposableNodes -from launch_ros.descriptions import ComposableNode -import yaml - - -def launch_setup(context, *args, **kwargs): - # vehicle information param path - vehicle_info_param_path = LaunchConfiguration("vehicle_param_file").perform(context) - with open(vehicle_info_param_path, "r") as f: - vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - # planning common param path - with open(LaunchConfiguration("common_param_path").perform(context), "r") as f: - common_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - # nearest search parameter - with open(LaunchConfiguration("nearest_search_param_path").perform(context), "r") as f: - nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - # path smoother - smoother_output_path_topic = "path_smoother/path" - with open(LaunchConfiguration("elastic_band_smoother_param_path").perform(context), "r") as f: - elastic_band_smoother_param = yaml.safe_load(f)["/**"]["ros__parameters"] - elastic_band_smoother_component = ComposableNode( - package="path_smoother", - plugin="path_smoother::ElasticBandSmoother", - name="elastic_band_smoother", - namespace="", - remappings=[ - ("~/input/path", LaunchConfiguration("input_path_topic")), - ("~/input/odometry", "/localization/kinematic_state"), - ("~/output/path", smoother_output_path_topic), - ], - parameters=[ - nearest_search_param, - elastic_band_smoother_param, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - planner_input_path_topic = PythonExpression( - [ - "'", - LaunchConfiguration("input_path_topic"), - "' if '", - LaunchConfiguration("path_smoother_type"), - "' == 'none'", - " else '", - smoother_output_path_topic, - "'", - ] - ) - # obstacle avoidance planner - with open( - LaunchConfiguration("obstacle_avoidance_planner_param_path").perform(context), "r" - ) as f: - obstacle_avoidance_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] - obstacle_avoidance_planner_component = ComposableNode( - package="obstacle_avoidance_planner", - plugin="obstacle_avoidance_planner::ObstacleAvoidancePlanner", - name="obstacle_avoidance_planner", - namespace="", - remappings=[ - ("~/input/path", planner_input_path_topic), - ("~/input/odometry", "/localization/kinematic_state"), - ("~/output/path", "obstacle_avoidance_planner/trajectory"), - ], - parameters=[ - nearest_search_param, - obstacle_avoidance_planner_param, - vehicle_info_param, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - # path sampler - with open(LaunchConfiguration("path_sampler_param_path").perform(context), "r") as f: - path_sampler_param = yaml.safe_load(f)["/**"]["ros__parameters"] - path_sampler_component = ComposableNode( - package="path_sampler", - plugin="path_sampler::PathSampler", - name="path_sampler", - namespace="", - remappings=[ - ("~/input/path", planner_input_path_topic), - ("~/input/odometry", "/localization/kinematic_state"), - ("~/output/path", "obstacle_avoidance_planner/trajectory"), - ], - parameters=[ - nearest_search_param, - path_sampler_param, - vehicle_info_param, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - # obstacle velocity limiter - with open( - LaunchConfiguration("obstacle_velocity_limiter_param_path").perform(context), "r" - ) as f: - obstacle_velocity_limiter_param = yaml.safe_load(f)["/**"]["ros__parameters"] - obstacle_velocity_limiter_component = ComposableNode( - package="obstacle_velocity_limiter", - plugin="obstacle_velocity_limiter::ObstacleVelocityLimiterNode", - name="obstacle_velocity_limiter", - namespace="", - remappings=[ - ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"), - ("~/input/odometry", "/localization/kinematic_state"), - ("~/input/dynamic_obstacles", "/perception/object_recognition/objects"), - ("~/input/occupancy_grid", "/perception/occupancy_grid_map/map"), - ("~/input/obstacle_pointcloud", "/perception/obstacle_segmentation/pointcloud"), - ("~/input/map", "/map/vector_map"), - ("~/output/debug_markers", "debug_markers"), - ("~/output/trajectory", "obstacle_velocity_limiter/trajectory"), - ], - parameters=[ - obstacle_velocity_limiter_param, - vehicle_info_param, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - # surround obstacle checker - with open( - LaunchConfiguration("surround_obstacle_checker_param_path").perform(context), "r" - ) as f: - surround_obstacle_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] - surround_obstacle_checker_component = ComposableNode( - package="surround_obstacle_checker", - plugin="surround_obstacle_checker::SurroundObstacleCheckerNode", - name="surround_obstacle_checker", - namespace="", - remappings=[ - ("~/output/no_start_reason", "/planning/scenario_planning/status/no_start_reason"), - ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"), - ("~/output/max_velocity", "/planning/scenario_planning/max_velocity_candidates"), - ( - "~/output/velocity_limit_clear_command", - "/planning/scenario_planning/clear_velocity_limit", - ), - ( - "~/input/pointcloud", - "/perception/obstacle_segmentation/pointcloud", - ), - ("~/input/objects", "/perception/object_recognition/objects"), - ("~/input/odometry", "/localization/kinematic_state"), - ], - parameters=[ - surround_obstacle_checker_param, - vehicle_info_param, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - # obstacle stop planner - with open(LaunchConfiguration("obstacle_stop_planner_param_path").perform(context), "r") as f: - obstacle_stop_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open( - LaunchConfiguration("obstacle_stop_planner_acc_param_path").perform(context), "r" - ) as f: - obstacle_stop_planner_acc_param = yaml.safe_load(f)["/**"]["ros__parameters"] - obstacle_stop_planner_component = ComposableNode( - package="obstacle_stop_planner", - plugin="motion_planning::ObstacleStopPlannerNode", - name="obstacle_stop_planner", - namespace="", - remappings=[ - ("~/output/stop_reason", "/planning/scenario_planning/status/stop_reason"), - ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"), - ("~/output/max_velocity", "/planning/scenario_planning/max_velocity_candidates"), - ( - "~/output/velocity_limit_clear_command", - "/planning/scenario_planning/clear_velocity_limit", - ), - ("~/output/trajectory", "/planning/scenario_planning/lane_driving/trajectory"), - ("~/input/acceleration", "/localization/acceleration"), - ( - "~/input/pointcloud", - "/perception/obstacle_segmentation/pointcloud", - ), - ("~/input/objects", "/perception/object_recognition/objects"), - ("~/input/odometry", "/localization/kinematic_state"), - ("~/input/trajectory", "obstacle_velocity_limiter/trajectory"), - ], - parameters=[ - nearest_search_param, - common_param, - obstacle_stop_planner_param, - obstacle_stop_planner_acc_param, - vehicle_info_param, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - # obstacle cruise planner - with open(LaunchConfiguration("obstacle_cruise_planner_param_path").perform(context), "r") as f: - obstacle_cruise_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] - obstacle_cruise_planner_component = ComposableNode( - package="obstacle_cruise_planner", - plugin="motion_planning::ObstacleCruisePlannerNode", - name="obstacle_cruise_planner", - namespace="", - remappings=[ - ("~/input/trajectory", "obstacle_velocity_limiter/trajectory"), - ("~/input/odometry", "/localization/kinematic_state"), - ("~/input/acceleration", "/localization/acceleration"), - ("~/input/objects", "/perception/object_recognition/objects"), - ("~/output/trajectory", "/planning/scenario_planning/lane_driving/trajectory"), - ("~/output/velocity_limit", "/planning/scenario_planning/max_velocity_candidates"), - ("~/output/clear_velocity_limit", "/planning/scenario_planning/clear_velocity_limit"), - ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"), - ], - parameters=[ - nearest_search_param, - common_param, - obstacle_cruise_planner_param, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - obstacle_cruise_planner_relay_component = ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="obstacle_cruise_planner_relay", - namespace="", - parameters=[ - {"input_topic": "obstacle_velocity_limiter/trajectory"}, - {"output_topic": "/planning/scenario_planning/lane_driving/trajectory"}, - {"type": "autoware_auto_planning_msgs/msg/Trajectory"}, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - container = ComposableNodeContainer( - name="motion_planning_container", - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=[ - obstacle_velocity_limiter_component, - ], - ) - - obstacle_stop_planner_loader = LoadComposableNodes( - composable_node_descriptions=[obstacle_stop_planner_component], - target_container=container, - condition=LaunchConfigurationEquals("cruise_planner_type", "obstacle_stop_planner"), - ) - - obstacle_cruise_planner_loader = LoadComposableNodes( - composable_node_descriptions=[obstacle_cruise_planner_component], - target_container=container, - condition=LaunchConfigurationEquals("cruise_planner_type", "obstacle_cruise_planner"), - ) - - obstacle_cruise_planner_relay_loader = LoadComposableNodes( - composable_node_descriptions=[obstacle_cruise_planner_relay_component], - target_container=container, - condition=LaunchConfigurationEquals("cruise_planner_type", "none"), - ) - - smoother_loader = LoadComposableNodes( - composable_node_descriptions=[elastic_band_smoother_component], - target_container=container, - condition=LaunchConfigurationEquals("path_smoother_type", "elastic_band"), - ) - - obstacle_avoidance_planner_loader = LoadComposableNodes( - composable_node_descriptions=[obstacle_avoidance_planner_component], - target_container=container, - condition=LaunchConfigurationEquals("path_planner_type", "obstacle_avoidance_planner"), - ) - - path_sampler_loader = LoadComposableNodes( - composable_node_descriptions=[path_sampler_component], - target_container=container, - condition=LaunchConfigurationEquals("path_planner_type", "path_sampler"), - ) - - surround_obstacle_checker_loader = LoadComposableNodes( - composable_node_descriptions=[surround_obstacle_checker_component], - target_container=container, - condition=IfCondition(LaunchConfiguration("use_surround_obstacle_check")), - ) - - glog_component = ComposableNode( - package="glog_component", - plugin="GlogComponent", - name="glog_component", - ) - glog_component_loader = LoadComposableNodes( - composable_node_descriptions=[glog_component], - target_container=container, - ) - - group = GroupAction( - [ - container, - smoother_loader, - obstacle_avoidance_planner_loader, - path_sampler_loader, - obstacle_stop_planner_loader, - obstacle_cruise_planner_loader, - obstacle_cruise_planner_relay_loader, - surround_obstacle_checker_loader, - glog_component_loader, - ] - ) - return [group] - - -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None, description=None): - launch_arguments.append( - DeclareLaunchArgument(name, default_value=default_value, description=description) - ) - - # vehicle parameter - add_launch_arg("vehicle_param_file") - - # interface parameter - add_launch_arg( - "input_path_topic", - "/planning/scenario_planning/lane_driving/behavior_planning/path", - "input path topic of obstacle_avoidance_planner", - ) - - # package parameter - add_launch_arg("use_surround_obstacle_check") # launch surround_obstacle_checker or not - add_launch_arg( - "cruise_planner_type" - ) # select from "obstacle_stop_planner", "obstacle_cruise_planner", "none" - add_launch_arg( - "path_planner_type", "obstacle_avoidance_planner" - ) # select from "obstacle_avoidance_planner", "path_sampler" - add_launch_arg("path_smoother_type", "elastic_band") # select from "elastic_band", "none" - - add_launch_arg("use_intra_process", "false", "use ROS 2 component container communication") - add_launch_arg("use_multithread", "false", "use multithread") - - 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 launch.LaunchDescription( - launch_arguments - + [ - set_container_executable, - set_container_mt_executable, - ] - + [OpaqueFunction(function=launch_setup)] - ) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index 3f7650486e69d..577b0e00d133c 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -1,42 +1,204 @@ - - - + + - - + + + - + - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py deleted file mode 100644 index 7dfcbb0307bf0..0000000000000 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py +++ /dev/null @@ -1,160 +0,0 @@ -# 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. - -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import GroupAction -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 PushRosNamespace -from launch_ros.descriptions import ComposableNode -from launch_ros.substitutions import FindPackageShare -import yaml - - -def launch_setup(context, *args, **kwargs): - vehicle_info_param_path = LaunchConfiguration("vehicle_param_file").perform(context) - with open(vehicle_info_param_path, "r") as f: - vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - with open(LaunchConfiguration("freespace_planner_param_path").perform(context), "r") as f: - freespace_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - container = ComposableNodeContainer( - name="parking_container", - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=[ - ComposableNode( - package="costmap_generator", - plugin="CostmapGenerator", - name="costmap_generator", - remappings=[ - ("~/input/objects", "/perception/object_recognition/objects"), - ( - "~/input/points_no_ground", - "/perception/obstacle_segmentation/pointcloud", - ), - ("~/input/vector_map", "/map/vector_map"), - ("~/input/scenario", "/planning/scenario_planning/scenario"), - ("~/output/grid_map", "costmap_generator/grid_map"), - ("~/output/occupancy_grid", "costmap_generator/occupancy_grid"), - ], - parameters=[ - { - "costmap_frame": "map", - "vehicle_frame": "base_link", - "map_frame": "map", - "update_rate": 10.0, - "activate_by_scenario": False, - "grid_min_value": 0.0, - "grid_max_value": 1.0, - "grid_resolution": 0.2, - "grid_length_x": 70.0, - "grid_length_y": 70.0, - "grid_position_x": 0.0, - "grid_position_y": 0.0, - "maximum_lidar_height_thres": 0.3, - "minimum_lidar_height_thres": -2.2, - "use_wayarea": True, - "use_parkinglot": True, - "use_objects": True, - "use_points": True, - "expand_polygon_size": 1.0, - "size_of_expansion_kernel": 9, - }, - vehicle_info_param, - ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ), - ComposableNode( - package="freespace_planner", - plugin="freespace_planner::FreespacePlannerNode", - name="freespace_planner", - remappings=[ - ("~/input/route", "/planning/mission_planning/route"), - ("~/input/occupancy_grid", "costmap_generator/occupancy_grid"), - ("~/input/scenario", "/planning/scenario_planning/scenario"), - ("~/input/odometry", "/localization/kinematic_state"), - ("~/output/trajectory", "/planning/scenario_planning/parking/trajectory"), - ("is_completed", "/planning/scenario_planning/parking/is_completed"), - ], - parameters=[ - freespace_planner_param, - vehicle_info_param, - ], - extra_arguments=[ - {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} - ], - ), - ], - ) - - group = GroupAction( - [ - PushRosNamespace("parking"), - container, - ] - ) - - return [group] - - -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None, description=None): - launch_arguments.append( - DeclareLaunchArgument(name, default_value=default_value, description=description) - ) - - add_launch_arg( - "vehicle_param_file", - [ - FindPackageShare("vehicle_info_util"), - "/config/vehicle_info.param.yaml", - ], - "path to the parameter file of vehicle information", - ) - - # component - add_launch_arg("use_intra_process", "false", "use ROS 2 component container communication") - add_launch_arg("use_multithread", "false", "use multithread") - - 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 launch.LaunchDescription( - launch_arguments - + [ - set_container_executable, - set_container_mt_executable, - ] - + [OpaqueFunction(function=launch_setup)] - ) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml index 741df95937c42..98315919b540a 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml @@ -1,32 +1,38 @@ - - - + - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml index b293c5836817d..66c90ef2ff833 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -26,7 +26,7 @@ - + @@ -50,20 +50,12 @@ - - - - - - - - - + - - + + diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 5988d34cded88..950ef67865a85 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -17,7 +17,9 @@ Takamasa Horibe Satoshi Ota - Yutaka Shimizu + Zulfaqar Azmi + Satoshi Ota + Mamoru Sobue Takayuki Murooka @@ -37,7 +39,8 @@ Mamoru Sobue - Yutaka Shimizu + Fumiya Watanabe + Zhe Shen Kosuke Takeuchi @@ -67,6 +70,7 @@ obstacle_cruise_planner obstacle_stop_planner planning_evaluator + planning_topic_converter planning_validator scenario_selector surround_obstacle_checker diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index 05c91bff13867..45d420bafff10 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -21,6 +21,7 @@ ament_auto_add_library(ekf_localizer_lib SHARED src/measurement.cpp src/state_transition.cpp src/warning_message.cpp + src/ekf_module.cpp ) target_link_libraries(ekf_localizer_lib Eigen3::Eigen) diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index 667217d2591dc..1c16624605907 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml @@ -24,9 +24,9 @@ # for diagnostics pose_no_update_count_threshold_warn: 50 - pose_no_update_count_threshold_error: 250 + pose_no_update_count_threshold_error: 100 twist_no_update_count_threshold_warn: 50 - twist_no_update_count_threshold_error: 250 + twist_no_update_count_threshold_error: 100 # for velocity measurement limitation (Set 0.0 if you want to ignore) threshold_observable_velocity_mps: 0.0 # [m/s] diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 4fc2305cc7adc..e2ffff2195645 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -16,13 +16,13 @@ #define EKF_LOCALIZER__EKF_LOCALIZER_HPP_ #include "ekf_localizer/aged_object_queue.hpp" +#include "ekf_localizer/ekf_module.hpp" #include "ekf_localizer/hyper_parameters.hpp" #include "ekf_localizer/warning.hpp" -#include -#include #include #include +#include #include #include @@ -89,7 +89,7 @@ class Simple1DFilter return; }; void set_proc_dev(const double proc_dev) { proc_dev_x_c_ = proc_dev; } - double get_x() { return x_; } + double get_x() const { return x_; } private: bool initialized_; @@ -105,7 +105,7 @@ class EKFLocalizer : public rclcpp::Node EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & options); private: - const Warning warning_; + const std::shared_ptr warning_; //!< @brief ekf estimated pose publisher rclcpp::Publisher::SharedPtr pub_pose_; @@ -117,10 +117,6 @@ class EKFLocalizer : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_twist_; //!< @brief ekf estimated twist with covariance publisher rclcpp::Publisher::SharedPtr pub_twist_cov_; - //!< @brief debug info publisher - rclcpp::Publisher::SharedPtr pub_debug_; - //!< @brief debug measurement pose publisher - rclcpp::Publisher::SharedPtr pub_measured_pose_; //!< @brief ekf estimated yaw bias publisher rclcpp::Publisher::SharedPtr pub_yaw_bias_; //!< @brief ekf estimated yaw bias publisher @@ -148,8 +144,11 @@ class EKFLocalizer : public rclcpp::Node //!< @brief tf broadcaster std::shared_ptr tf_br_; + //!< @brief logger configure module + std::unique_ptr logger_configure_; + //!< @brief extended kalman filter instance. - TimeDelayKalmanFilter ekf_; + std::unique_ptr ekf_module_; Simple1DFilter z_filter_; Simple1DFilter roll_filter_; Simple1DFilter pitch_filter_; @@ -159,10 +158,6 @@ class EKFLocalizer : public rclcpp::Node double ekf_rate_; double ekf_dt_; - /* parameters */ - - int dim_x_; //!< @brief dimension of EKF state - /* process noise variance for discrete model */ double proc_cov_yaw_d_; //!< @brief discrete yaw process noise double proc_cov_yaw_bias_d_; //!< @brief discrete yaw bias process noise @@ -171,32 +166,12 @@ class EKFLocalizer : public rclcpp::Node bool is_activated_; - size_t pose_no_update_count_; - size_t pose_queue_size_; - bool pose_is_passed_delay_gate_; - double pose_delay_time_; - double pose_delay_time_threshold_; - bool pose_is_passed_mahalanobis_gate_; - double pose_mahalanobis_distance_; - - size_t twist_no_update_count_; - size_t twist_queue_size_; - bool twist_is_passed_delay_gate_; - double twist_delay_time_; - double twist_delay_time_threshold_; - bool twist_is_passed_mahalanobis_gate_; - double twist_mahalanobis_distance_; + EKFDiagnosticInfo pose_diag_info_; + EKFDiagnosticInfo twist_diag_info_; AgedObjectQueue pose_queue_; AgedObjectQueue twist_queue_; - geometry_msgs::msg::PoseStamped current_ekf_pose_; //!< @brief current estimated pose - geometry_msgs::msg::PoseStamped - current_biased_ekf_pose_; //!< @brief current estimated pose without yaw bias correction - geometry_msgs::msg::TwistStamped current_ekf_twist_; //!< @brief current estimated twist - std::array current_pose_covariance_; - std::array current_twist_covariance_; - /** * @brief computes update & prediction of EKF for each ekf_dt_[s] time */ @@ -222,33 +197,11 @@ class EKFLocalizer : public rclcpp::Node */ void callbackInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - /** - * @brief initialization of EKF - */ - void initEKF(); - /** * @brief update predict frequency */ void updatePredictFrequency(); - /** - * @brief compute EKF prediction - */ - void predictKinematicsModel(); - - /** - * @brief compute EKF update with pose measurement - * @param pose measurement value - */ - bool measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); - - /** - * @brief compute EKF update with pose measurement - * @param twist measurement value - */ - bool measurementUpdateTwist(const geometry_msgs::msg::TwistWithCovarianceStamped & twist); - /** * @brief get transform from frame_id */ @@ -256,26 +209,19 @@ class EKFLocalizer : public rclcpp::Node std::string parent_frame, std::string child_frame, geometry_msgs::msg::TransformStamped & transform); - /** - * @brief set current EKF estimation result to current_ekf_pose_ & current_ekf_twist_ - */ - void setCurrentResult(); - /** * @brief publish current EKF estimation result */ - void publishEstimateResult(); + void publishEstimateResult( + const geometry_msgs::msg::PoseStamped & current_ekf_pose, + const geometry_msgs::msg::PoseStamped & current_biased_ekf_pose, + const geometry_msgs::msg::TwistStamped & current_ekf_twist); /** * @brief publish diagnostics message */ void publishDiagnostics(); - /** - * @brief for debug - */ - void showCurrentX(); - /** * @brief update simple1DFilter */ diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp new file mode 100644 index 0000000000000..873060c75fcfc --- /dev/null +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp @@ -0,0 +1,97 @@ +// Copyright 2018-2019 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. + +#ifndef EKF_LOCALIZER__EKF_MODULE_HPP_ +#define EKF_LOCALIZER__EKF_MODULE_HPP_ + +#include "ekf_localizer/hyper_parameters.hpp" +#include "ekf_localizer/state_index.hpp" +#include "ekf_localizer/warning.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +struct EKFDiagnosticInfo +{ + EKFDiagnosticInfo() + : no_update_count(0), + queue_size(0), + is_passed_delay_gate(true), + delay_time(0), + delay_time_threshold(0), + is_passed_mahalanobis_gate(true), + mahalanobis_distance(0) + { + } + + size_t no_update_count; + size_t queue_size; + bool is_passed_delay_gate; + double delay_time; + double delay_time_threshold; + bool is_passed_mahalanobis_gate; + double mahalanobis_distance; +}; + +class EKFModule +{ +private: + using PoseWithCovariance = geometry_msgs::msg::PoseWithCovarianceStamped; + using TwistWithCovariance = geometry_msgs::msg::TwistWithCovarianceStamped; + using Pose = geometry_msgs::msg::PoseStamped; + using Twist = geometry_msgs::msg::TwistStamped; + +public: + EKFModule(std::shared_ptr warning, const HyperParameters params); + + void initialize( + const PoseWithCovariance & initial_pose, + const geometry_msgs::msg::TransformStamped & transform); + + geometry_msgs::msg::PoseStamped getCurrentPose( + const rclcpp::Time & current_time, const double z, const double roll, const double pitch, + bool get_biased_yaw) const; + geometry_msgs::msg::TwistStamped getCurrentTwist(const rclcpp::Time & current_time) const; + double getYawBias() const; + std::array getCurrentPoseCovariance() const; + std::array getCurrentTwistCovariance() const; + + void predictWithDelay(const double dt); + bool measurementUpdatePose( + const PoseWithCovariance & pose, const double dt, const rclcpp::Time & t_curr, + EKFDiagnosticInfo & pose_diag_info); + bool measurementUpdateTwist( + const TwistWithCovariance & twist, const double dt, const rclcpp::Time & t_curr, + EKFDiagnosticInfo & twist_diag_info); + geometry_msgs::msg::PoseWithCovarianceStamped compensatePoseWithZDelay( + const PoseWithCovariance & pose, const double delay_time); + +private: + TimeDelayKalmanFilter kalman_filter_; + + std::shared_ptr warning_; + const int dim_x_; + const HyperParameters params_; +}; + +#endif // EKF_LOCALIZER__EKF_MODULE_HPP_ diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index b39112b1d8d62..5dd78a00f2fbd 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -14,16 +14,8 @@ #include "ekf_localizer/ekf_localizer.hpp" -#include "ekf_localizer/covariance.hpp" #include "ekf_localizer/diagnostics.hpp" -#include "ekf_localizer/mahalanobis.hpp" -#include "ekf_localizer/matrix_types.hpp" -#include "ekf_localizer/measurement.hpp" -#include "ekf_localizer/numeric.hpp" -#include "ekf_localizer/state_index.hpp" -#include "ekf_localizer/state_transition.hpp" #include "ekf_localizer/string.hpp" -#include "ekf_localizer/warning.hpp" #include "ekf_localizer/warning_message.hpp" #include @@ -44,20 +36,16 @@ // clang-format off #define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl #define DEBUG_INFO(...) {if (params_.show_debug_info) {RCLCPP_INFO(__VA_ARGS__);}} -#define DEBUG_PRINT_MAT(X) {\ - if (params_.show_debug_info) {std::cout << #X << ": " << X << std::endl;}\ -} // clang-format on using std::placeholders::_1; EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & node_options) : rclcpp::Node(node_name, node_options), - warning_(this), + warning_(std::make_shared(this)), params_(this), ekf_rate_(params_.ekf_rate), ekf_dt_(params_.ekf_dt), - dim_x_(6 /* x, y, yaw, yaw_bias, vx, wz */), pose_queue_(params_.pose_smoothing_steps), twist_queue_(params_.twist_smoothing_steps) { @@ -104,15 +92,12 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti tf_br_ = std::make_shared( std::shared_ptr(this, [](auto) {})); - initEKF(); + ekf_module_ = std::make_unique(warning_, params_); + logger_configure_ = std::make_unique(this); z_filter_.set_proc_dev(1.0); roll_filter_.set_proc_dev(0.01); pitch_filter_.set_proc_dev(0.01); - - /* debug */ - pub_debug_ = create_publisher("debug", 1); - pub_measured_pose_ = create_publisher("debug/measured_pose", 1); } /* @@ -122,7 +107,7 @@ void EKFLocalizer::updatePredictFrequency() { if (last_predict_time_) { if (get_clock()->now() < *last_predict_time_) { - warning_.warn("Detected jump back in time"); + warning_->warn("Detected jump back in time"); } else { ekf_rate_ = 1.0 / (get_clock()->now() - *last_predict_time_).seconds(); DEBUG_INFO(get_logger(), "[EKF] update ekf_rate_ to %f hz", ekf_rate_); @@ -143,7 +128,7 @@ void EKFLocalizer::updatePredictFrequency() void EKFLocalizer::timerCallback() { if (!is_activated_) { - warning_.warnThrottle( + warning_->warnThrottle( "The node is not activated. Provide initial pose to pose_initializer", 2000); publishDiagnostics(); return; @@ -157,35 +142,17 @@ void EKFLocalizer::timerCallback() /* predict model in EKF */ stop_watch_.tic(); DEBUG_INFO(get_logger(), "------------------------- start prediction -------------------------"); - - const Eigen::MatrixXd X_curr = ekf_.getLatestX(); - DEBUG_PRINT_MAT(X_curr.transpose()); - - const Eigen::MatrixXd P_curr = ekf_.getLatestP(); - - const double dt = ekf_dt_; - - const Vector6d X_next = predictNextState(X_curr, dt); - const Matrix6d A = createStateTransitionMatrix(X_curr, dt); - const Matrix6d Q = processNoiseCovariance(proc_cov_yaw_d_, proc_cov_vx_d_, proc_cov_wz_d_); - - ekf_.predictWithDelay(X_next, A, Q); - - // debug - const Eigen::MatrixXd X_result = ekf_.getLatestX(); - DEBUG_PRINT_MAT(X_result.transpose()); - DEBUG_PRINT_MAT((X_result - X_curr).transpose()); + ekf_module_->predictWithDelay(ekf_dt_); DEBUG_INFO(get_logger(), "[EKF] predictKinematicsModel calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end prediction -------------------------\n"); /* pose measurement update */ - - pose_queue_size_ = pose_queue_.size(); - pose_is_passed_delay_gate_ = true; - pose_delay_time_ = 0.0; - pose_delay_time_threshold_ = 0.0; - pose_is_passed_mahalanobis_gate_ = true; - pose_mahalanobis_distance_ = 0.0; + pose_diag_info_.queue_size = pose_queue_.size(); + pose_diag_info_.is_passed_delay_gate = true; + pose_diag_info_.delay_time = 0.0; + pose_diag_info_.delay_time_threshold = 0.0; + pose_diag_info_.is_passed_mahalanobis_gate = true; + pose_diag_info_.mahalanobis_distance = 0.0; bool pose_is_updated = false; @@ -194,27 +161,33 @@ void EKFLocalizer::timerCallback() stop_watch_.tic(); // save the initial size because the queue size can change in the loop + const auto t_curr = this->now(); const size_t n = pose_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto pose = pose_queue_.pop_increment_age(); - bool is_updated = measurementUpdatePose(*pose); + bool is_updated = ekf_module_->measurementUpdatePose(*pose, ekf_dt_, t_curr, pose_diag_info_); if (is_updated) { pose_is_updated = true; + + // Update Simple 1D filter with considering change of z value due to measurement pose delay + const double delay_time = + (t_curr - pose->header.stamp).seconds() + params_.pose_additional_delay; + const auto pose_with_z_delay = ekf_module_->compensatePoseWithZDelay(*pose, delay_time); + updateSimple1DFilters(pose_with_z_delay, params_.pose_smoothing_steps); } } DEBUG_INFO(get_logger(), "[EKF] measurementUpdatePose calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end Pose -------------------------\n"); } - pose_no_update_count_ = pose_is_updated ? 0 : (pose_no_update_count_ + 1); + pose_diag_info_.no_update_count = pose_is_updated ? 0 : (pose_diag_info_.no_update_count + 1); /* twist measurement update */ - - twist_queue_size_ = twist_queue_.size(); - twist_is_passed_delay_gate_ = true; - twist_delay_time_ = 0.0; - twist_delay_time_threshold_ = 0.0; - twist_is_passed_mahalanobis_gate_ = true; - twist_mahalanobis_distance_ = 0.0; + twist_diag_info_.queue_size = twist_queue_.size(); + twist_diag_info_.is_passed_delay_gate = true; + twist_diag_info_.delay_time = 0.0; + twist_diag_info_.delay_time_threshold = 0.0; + twist_diag_info_.is_passed_mahalanobis_gate = true; + twist_diag_info_.mahalanobis_distance = 0.0; bool twist_is_updated = false; @@ -223,10 +196,12 @@ void EKFLocalizer::timerCallback() stop_watch_.tic(); // save the initial size because the queue size can change in the loop + const auto t_curr = this->now(); const size_t n = twist_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto twist = twist_queue_.pop_increment_age(); - bool is_updated = measurementUpdateTwist(*twist); + bool is_updated = + ekf_module_->measurementUpdateTwist(*twist, ekf_dt_, t_curr, twist_diag_info_); if (is_updated) { twist_is_updated = true; } @@ -234,49 +209,23 @@ void EKFLocalizer::timerCallback() DEBUG_INFO(get_logger(), "[EKF] measurementUpdateTwist calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end Twist -------------------------\n"); } - twist_no_update_count_ = twist_is_updated ? 0 : (twist_no_update_count_ + 1); + twist_diag_info_.no_update_count = twist_is_updated ? 0 : (twist_diag_info_.no_update_count + 1); - const double x = ekf_.getXelement(IDX::X); - const double y = ekf_.getXelement(IDX::Y); const double z = z_filter_.get_x(); - - const double biased_yaw = ekf_.getXelement(IDX::YAW); - const double yaw_bias = ekf_.getXelement(IDX::YAWB); - const double roll = roll_filter_.get_x(); const double pitch = pitch_filter_.get_x(); - const double yaw = biased_yaw + yaw_bias; - const double vx = ekf_.getXelement(IDX::VX); - const double wz = ekf_.getXelement(IDX::WZ); - - current_ekf_pose_.header.frame_id = params_.pose_frame_id; - current_ekf_pose_.header.stamp = this->now(); - current_ekf_pose_.pose.position = tier4_autoware_utils::createPoint(x, y, z); - current_ekf_pose_.pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, yaw); - - current_biased_ekf_pose_ = current_ekf_pose_; - current_biased_ekf_pose_.pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, biased_yaw); - - current_ekf_twist_.header.frame_id = "base_link"; - current_ekf_twist_.header.stamp = this->now(); - current_ekf_twist_.twist.linear.x = vx; - current_ekf_twist_.twist.angular.z = wz; + const geometry_msgs::msg::PoseStamped current_ekf_pose = + ekf_module_->getCurrentPose(this->now(), z, roll, pitch, false); + const geometry_msgs::msg::PoseStamped current_biased_ekf_pose = + ekf_module_->getCurrentPose(this->now(), z, roll, pitch, true); + const geometry_msgs::msg::TwistStamped current_ekf_twist = + ekf_module_->getCurrentTwist(this->now()); /* publish ekf result */ - publishEstimateResult(); + publishEstimateResult(current_ekf_pose, current_biased_ekf_pose, current_ekf_twist); publishDiagnostics(); } -void EKFLocalizer::showCurrentX() -{ - if (params_.show_debug_info) { - const Eigen::MatrixXd X = ekf_.getLatestX(); - DEBUG_PRINT_MAT(X.transpose()); - } -} - /* * timerTFCallback */ @@ -286,12 +235,17 @@ void EKFLocalizer::timerTFCallback() return; } - if (current_ekf_pose_.header.frame_id == "") { + if (params_.pose_frame_id == "") { return; } + const double z = z_filter_.get_x(); + const double roll = roll_filter_.get_x(); + const double pitch = pitch_filter_.get_x(); + geometry_msgs::msg::TransformStamped transform_stamped; - transform_stamped = tier4_autoware_utils::pose2transform(current_ekf_pose_, "base_link"); + transform_stamped = tier4_autoware_utils::pose2transform( + ekf_module_->getCurrentPose(this->now(), z, roll, pitch, false), "base_link"); transform_stamped.header.stamp = this->now(); tf_br_->sendTransform(transform_stamped); } @@ -315,7 +269,7 @@ bool EKFLocalizer::getTransformFromTF( transform = tf_buffer.lookupTransform(parent_frame, child_frame, tf2::TimePointZero); return true; } catch (tf2::TransformException & ex) { - warning_.warn(ex.what()); + warning_->warn(ex.what()); rclcpp::sleep_for(std::chrono::milliseconds(100)); } } @@ -334,35 +288,7 @@ void EKFLocalizer::callbackInitialPose( get_logger(), "[EKF] TF transform failed. parent = %s, child = %s", params_.pose_frame_id.c_str(), initialpose->header.frame_id.c_str()); } - - Eigen::MatrixXd X(dim_x_, 1); - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x_, dim_x_); - - // TODO(mitsudome-r) need mutex - - X(IDX::X) = initialpose->pose.pose.position.x + transform.transform.translation.x; - X(IDX::Y) = initialpose->pose.pose.position.y + transform.transform.translation.y; - current_ekf_pose_.pose.position.z = - initialpose->pose.pose.position.z + transform.transform.translation.z; - X(IDX::YAW) = - tf2::getYaw(initialpose->pose.pose.orientation) + tf2::getYaw(transform.transform.rotation); - X(IDX::YAWB) = 0.0; - X(IDX::VX) = 0.0; - X(IDX::WZ) = 0.0; - - using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - P(IDX::X, IDX::X) = initialpose->pose.covariance[COV_IDX::X_X]; - P(IDX::Y, IDX::Y) = initialpose->pose.covariance[COV_IDX::Y_Y]; - P(IDX::YAW, IDX::YAW) = initialpose->pose.covariance[COV_IDX::YAW_YAW]; - - if (params_.enable_yaw_bias_estimation) { - P(IDX::YAWB, IDX::YAWB) = 0.0001; - } - P(IDX::VX, IDX::VX) = 0.01; - P(IDX::WZ, IDX::WZ) = 0.01; - - ekf_.init(X, P, params_.extend_state_step); - + ekf_module_->initialize(*initialpose, transform); initSimple1DFilters(*initialpose); } @@ -393,271 +319,57 @@ void EKFLocalizer::callbackTwistWithCovariance( twist_queue_.push(msg); } -/* - * initEKF - */ -void EKFLocalizer::initEKF() -{ - Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_x_, 1); - Eigen::MatrixXd P = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15; // for x & y - P(IDX::YAW, IDX::YAW) = 50.0; // for yaw - if (params_.enable_yaw_bias_estimation) { - P(IDX::YAWB, IDX::YAWB) = 50.0; // for yaw bias - } - P(IDX::VX, IDX::VX) = 1000.0; // for vx - P(IDX::WZ, IDX::WZ) = 50.0; // for wz - - ekf_.init(X, P, params_.extend_state_step); -} - -/* - * measurementUpdatePose - */ -bool EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) -{ - if (pose.header.frame_id != params_.pose_frame_id) { - warning_.warnThrottle( - fmt::format( - "pose frame_id is %s, but pose_frame is set as %s. They must be same.", - pose.header.frame_id.c_str(), params_.pose_frame_id.c_str()), - 2000); - } - const Eigen::MatrixXd X_curr = ekf_.getLatestX(); - DEBUG_PRINT_MAT(X_curr.transpose()); - - constexpr int dim_y = 3; // pos_x, pos_y, yaw, depending on Pose output - const rclcpp::Time t_curr = this->now(); - - /* Calculate delay step */ - double delay_time = (t_curr - pose.header.stamp).seconds() + params_.pose_additional_delay; - if (delay_time < 0.0) { - warning_.warnThrottle(poseDelayTimeWarningMessage(delay_time), 1000); - } - - delay_time = std::max(delay_time, 0.0); - - int delay_step = std::roundf(delay_time / ekf_dt_); - - pose_delay_time_ = std::max(delay_time, pose_delay_time_); - pose_delay_time_threshold_ = params_.extend_state_step * ekf_dt_; - if (delay_step >= params_.extend_state_step) { - pose_is_passed_delay_gate_ = false; - warning_.warnThrottle( - poseDelayStepWarningMessage(delay_time, params_.extend_state_step, ekf_dt_), 2000); - return false; - } - DEBUG_INFO(get_logger(), "delay_time: %f [s]", delay_time); - - /* Set yaw */ - double yaw = tf2::getYaw(pose.pose.pose.orientation); - const double ekf_yaw = ekf_.getXelement(delay_step * dim_x_ + IDX::YAW); - const double yaw_error = normalizeYaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi - yaw = yaw_error + ekf_yaw; - - /* Set measurement matrix */ - Eigen::MatrixXd y(dim_y, 1); - y << pose.pose.pose.position.x, pose.pose.pose.position.y, yaw; - - if (hasNan(y) || hasInf(y)) { - warning_.warn( - "[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message."); - return false; - } - - /* Gate */ - const Eigen::Vector3d y_ekf( - ekf_.getXelement(delay_step * dim_x_ + IDX::X), ekf_.getXelement(delay_step * dim_x_ + IDX::Y), - ekf_yaw); - const Eigen::MatrixXd P_curr = ekf_.getLatestP(); - const Eigen::MatrixXd P_y = P_curr.block(0, 0, dim_y, dim_y); - - const double distance = mahalanobis(y_ekf, y, P_y); - pose_mahalanobis_distance_ = std::max(distance, pose_mahalanobis_distance_); - if (distance > params_.pose_gate_dist) { - pose_is_passed_mahalanobis_gate_ = false; - warning_.warnThrottle(mahalanobisWarningMessage(distance, params_.pose_gate_dist), 2000); - warning_.warnThrottle("Ignore the measurement data.", 2000); - return false; - } - - DEBUG_PRINT_MAT(y.transpose()); - DEBUG_PRINT_MAT(y_ekf.transpose()); - DEBUG_PRINT_MAT((y - y_ekf).transpose()); - - const Eigen::Matrix C = poseMeasurementMatrix(); - const Eigen::Matrix3d R = - poseMeasurementCovariance(pose.pose.covariance, params_.pose_smoothing_steps); - - ekf_.updateWithDelay(y, C, R, delay_step); - - // Considering change of z value due to measurement pose delay - const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation); - const double dz_delay = current_ekf_twist_.twist.linear.x * delay_time * std::sin(-rpy.y); - geometry_msgs::msg::PoseWithCovarianceStamped pose_with_z_delay; - pose_with_z_delay = pose; - pose_with_z_delay.pose.pose.position.z += dz_delay; - - updateSimple1DFilters(pose_with_z_delay, params_.pose_smoothing_steps); - - // debug - const Eigen::MatrixXd X_result = ekf_.getLatestX(); - DEBUG_PRINT_MAT(X_result.transpose()); - DEBUG_PRINT_MAT((X_result - X_curr).transpose()); - - return true; -} - -/* - * measurementUpdateTwist - */ -bool EKFLocalizer::measurementUpdateTwist( - const geometry_msgs::msg::TwistWithCovarianceStamped & twist) -{ - if (twist.header.frame_id != "base_link") { - RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(2000).count(), - "twist frame_id must be base_link"); - } - - const Eigen::MatrixXd X_curr = ekf_.getLatestX(); - DEBUG_PRINT_MAT(X_curr.transpose()); - - constexpr int dim_y = 2; // vx, wz - const rclcpp::Time t_curr = this->now(); - - /* Calculate delay step */ - double delay_time = (t_curr - twist.header.stamp).seconds() + params_.twist_additional_delay; - if (delay_time < 0.0) { - warning_.warnThrottle(twistDelayTimeWarningMessage(delay_time), 1000); - } - delay_time = std::max(delay_time, 0.0); - - int delay_step = std::roundf(delay_time / ekf_dt_); - - twist_delay_time_ = std::max(delay_time, twist_delay_time_); - twist_delay_time_threshold_ = params_.extend_state_step * ekf_dt_; - if (delay_step >= params_.extend_state_step) { - twist_is_passed_delay_gate_ = false; - warning_.warnThrottle( - twistDelayStepWarningMessage(delay_time, params_.extend_state_step, ekf_dt_), 2000); - return false; - } - DEBUG_INFO(get_logger(), "delay_time: %f [s]", delay_time); - - /* Set measurement matrix */ - Eigen::MatrixXd y(dim_y, 1); - y << twist.twist.twist.linear.x, twist.twist.twist.angular.z; - - if (hasNan(y) || hasInf(y)) { - warning_.warn( - "[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message."); - return false; - } - - const Eigen::Vector2d y_ekf( - ekf_.getXelement(delay_step * dim_x_ + IDX::VX), - ekf_.getXelement(delay_step * dim_x_ + IDX::WZ)); - const Eigen::MatrixXd P_curr = ekf_.getLatestP(); - const Eigen::MatrixXd P_y = P_curr.block(4, 4, dim_y, dim_y); - - const double distance = mahalanobis(y_ekf, y, P_y); - twist_mahalanobis_distance_ = std::max(distance, twist_mahalanobis_distance_); - if (distance > params_.twist_gate_dist) { - twist_is_passed_mahalanobis_gate_ = false; - warning_.warnThrottle(mahalanobisWarningMessage(distance, params_.twist_gate_dist), 2000); - warning_.warnThrottle("Ignore the measurement data.", 2000); - return false; - } - - DEBUG_PRINT_MAT(y.transpose()); - DEBUG_PRINT_MAT(y_ekf.transpose()); - DEBUG_PRINT_MAT((y - y_ekf).transpose()); - - const Eigen::Matrix C = twistMeasurementMatrix(); - const Eigen::Matrix2d R = - twistMeasurementCovariance(twist.twist.covariance, params_.twist_smoothing_steps); - - ekf_.updateWithDelay(y, C, R, delay_step); - - // debug - const Eigen::MatrixXd X_result = ekf_.getLatestX(); - DEBUG_PRINT_MAT(X_result.transpose()); - DEBUG_PRINT_MAT((X_result - X_curr).transpose()); - - return true; -} - /* * publishEstimateResult */ -void EKFLocalizer::publishEstimateResult() +void EKFLocalizer::publishEstimateResult( + const geometry_msgs::msg::PoseStamped & current_ekf_pose, + const geometry_msgs::msg::PoseStamped & current_biased_ekf_pose, + const geometry_msgs::msg::TwistStamped & current_ekf_twist) { rclcpp::Time current_time = this->now(); - const Eigen::MatrixXd X = ekf_.getLatestX(); - const Eigen::MatrixXd P = ekf_.getLatestP(); /* publish latest pose */ - pub_pose_->publish(current_ekf_pose_); - pub_biased_pose_->publish(current_biased_ekf_pose_); + pub_pose_->publish(current_ekf_pose); + pub_biased_pose_->publish(current_biased_ekf_pose); /* publish latest pose with covariance */ geometry_msgs::msg::PoseWithCovarianceStamped pose_cov; pose_cov.header.stamp = current_time; - pose_cov.header.frame_id = current_ekf_pose_.header.frame_id; - pose_cov.pose.pose = current_ekf_pose_.pose; - pose_cov.pose.covariance = ekfCovarianceToPoseMessageCovariance(P); + pose_cov.header.frame_id = current_ekf_pose.header.frame_id; + pose_cov.pose.pose = current_ekf_pose.pose; + pose_cov.pose.covariance = ekf_module_->getCurrentPoseCovariance(); pub_pose_cov_->publish(pose_cov); geometry_msgs::msg::PoseWithCovarianceStamped biased_pose_cov = pose_cov; - biased_pose_cov.pose.pose = current_biased_ekf_pose_.pose; + biased_pose_cov.pose.pose = current_biased_ekf_pose.pose; pub_biased_pose_cov_->publish(biased_pose_cov); /* publish latest twist */ - pub_twist_->publish(current_ekf_twist_); + pub_twist_->publish(current_ekf_twist); /* publish latest twist with covariance */ geometry_msgs::msg::TwistWithCovarianceStamped twist_cov; twist_cov.header.stamp = current_time; - twist_cov.header.frame_id = current_ekf_twist_.header.frame_id; - twist_cov.twist.twist = current_ekf_twist_.twist; - twist_cov.twist.covariance = ekfCovarianceToTwistMessageCovariance(P); + twist_cov.header.frame_id = current_ekf_twist.header.frame_id; + twist_cov.twist.twist = current_ekf_twist.twist; + twist_cov.twist.covariance = ekf_module_->getCurrentTwistCovariance(); pub_twist_cov_->publish(twist_cov); /* publish yaw bias */ tier4_debug_msgs::msg::Float64Stamped yawb; yawb.stamp = current_time; - yawb.data = X(IDX::YAWB); + yawb.data = ekf_module_->getYawBias(); pub_yaw_bias_->publish(yawb); /* publish latest odometry */ nav_msgs::msg::Odometry odometry; odometry.header.stamp = current_time; - odometry.header.frame_id = current_ekf_pose_.header.frame_id; + odometry.header.frame_id = current_ekf_pose.header.frame_id; odometry.child_frame_id = "base_link"; odometry.pose = pose_cov.pose; odometry.twist = twist_cov.twist; pub_odom_->publish(odometry); - - /* debug measured pose */ - if (!pose_queue_.empty()) { - geometry_msgs::msg::PoseStamped p; - p.pose = pose_queue_.back()->pose.pose; - p.header.stamp = current_time; - pub_measured_pose_->publish(p); - } - - /* debug publish */ - double pose_yaw = 0.0; - if (!pose_queue_.empty()) { - pose_yaw = tf2::getYaw(pose_queue_.back()->pose.pose.orientation); - } - - tier4_debug_msgs::msg::Float64MultiArrayStamped msg; - msg.stamp = current_time; - msg.data.push_back(tier4_autoware_utils::rad2deg(X(IDX::YAW))); // [0] ekf yaw angle - msg.data.push_back(tier4_autoware_utils::rad2deg(pose_yaw)); // [1] measurement yaw angle - msg.data.push_back(tier4_autoware_utils::rad2deg(X(IDX::YAWB))); // [2] yaw bias - pub_debug_->publish(msg); } void EKFLocalizer::publishDiagnostics() @@ -668,23 +380,25 @@ void EKFLocalizer::publishDiagnostics() if (is_activated_) { diag_status_array.push_back(checkMeasurementUpdated( - "pose", pose_no_update_count_, params_.pose_no_update_count_threshold_warn, + "pose", pose_diag_info_.no_update_count, params_.pose_no_update_count_threshold_warn, params_.pose_no_update_count_threshold_error)); - diag_status_array.push_back(checkMeasurementQueueSize("pose", pose_queue_size_)); + diag_status_array.push_back(checkMeasurementQueueSize("pose", pose_diag_info_.queue_size)); diag_status_array.push_back(checkMeasurementDelayGate( - "pose", pose_is_passed_delay_gate_, pose_delay_time_, pose_delay_time_threshold_)); + "pose", pose_diag_info_.is_passed_delay_gate, pose_diag_info_.delay_time, + pose_diag_info_.delay_time_threshold)); diag_status_array.push_back(checkMeasurementMahalanobisGate( - "pose", pose_is_passed_mahalanobis_gate_, pose_mahalanobis_distance_, + "pose", pose_diag_info_.is_passed_mahalanobis_gate, pose_diag_info_.mahalanobis_distance, params_.pose_gate_dist)); diag_status_array.push_back(checkMeasurementUpdated( - "twist", twist_no_update_count_, params_.twist_no_update_count_threshold_warn, + "twist", twist_diag_info_.no_update_count, params_.twist_no_update_count_threshold_warn, params_.twist_no_update_count_threshold_error)); - diag_status_array.push_back(checkMeasurementQueueSize("twist", twist_queue_size_)); + diag_status_array.push_back(checkMeasurementQueueSize("twist", twist_diag_info_.queue_size)); diag_status_array.push_back(checkMeasurementDelayGate( - "twist", twist_is_passed_delay_gate_, twist_delay_time_, twist_delay_time_threshold_)); + "twist", twist_diag_info_.is_passed_delay_gate, twist_diag_info_.delay_time, + twist_diag_info_.delay_time_threshold)); diag_status_array.push_back(checkMeasurementMahalanobisGate( - "twist", twist_is_passed_mahalanobis_gate_, twist_mahalanobis_distance_, + "twist", twist_diag_info_.is_passed_mahalanobis_gate, twist_diag_info_.mahalanobis_distance, params_.twist_gate_dist)); } diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp new file mode 100644 index 0000000000000..0a5e3c98c96c8 --- /dev/null +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -0,0 +1,317 @@ +// Copyright 2018-2019 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 "ekf_localizer/ekf_module.hpp" + +#include "ekf_localizer/covariance.hpp" +#include "ekf_localizer/mahalanobis.hpp" +#include "ekf_localizer/matrix_types.hpp" +#include "ekf_localizer/measurement.hpp" +#include "ekf_localizer/numeric.hpp" +#include "ekf_localizer/state_transition.hpp" +#include "ekf_localizer/warning_message.hpp" + +#include +#include + +#include +#include +#include + +// clang-format off +#define DEBUG_PRINT_MAT(X) {\ + if (params_.show_debug_info) {std::cout << #X << ": " << X << std::endl;}\ +} +// clang-format on + +EKFModule::EKFModule(std::shared_ptr warning, const HyperParameters params) +: warning_(std::move(warning)), + dim_x_(6), // x, y, yaw, yaw_bias, vx, wz + params_(params) +{ + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_x_, 1); + Eigen::MatrixXd P = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15; // for x & y + P(IDX::YAW, IDX::YAW) = 50.0; // for yaw + if (params_.enable_yaw_bias_estimation) { + P(IDX::YAWB, IDX::YAWB) = 50.0; // for yaw bias + } + P(IDX::VX, IDX::VX) = 1000.0; // for vx + P(IDX::WZ, IDX::WZ) = 50.0; // for wz + + kalman_filter_.init(X, P, params_.extend_state_step); +} + +void EKFModule::initialize( + const PoseWithCovariance & initial_pose, const geometry_msgs::msg::TransformStamped & transform) +{ + Eigen::MatrixXd X(dim_x_, 1); + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x_, dim_x_); + + X(IDX::X) = initial_pose.pose.pose.position.x + transform.transform.translation.x; + X(IDX::Y) = initial_pose.pose.pose.position.y + transform.transform.translation.y; + X(IDX::YAW) = + tf2::getYaw(initial_pose.pose.pose.orientation) + tf2::getYaw(transform.transform.rotation); + X(IDX::YAWB) = 0.0; + X(IDX::VX) = 0.0; + X(IDX::WZ) = 0.0; + + using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + P(IDX::X, IDX::X) = initial_pose.pose.covariance[COV_IDX::X_X]; + P(IDX::Y, IDX::Y) = initial_pose.pose.covariance[COV_IDX::Y_Y]; + P(IDX::YAW, IDX::YAW) = initial_pose.pose.covariance[COV_IDX::YAW_YAW]; + + if (params_.enable_yaw_bias_estimation) { + P(IDX::YAWB, IDX::YAWB) = 0.0001; + } + P(IDX::VX, IDX::VX) = 0.01; + P(IDX::WZ, IDX::WZ) = 0.01; + + kalman_filter_.init(X, P, params_.extend_state_step); +} + +geometry_msgs::msg::PoseStamped EKFModule::getCurrentPose( + const rclcpp::Time & current_time, const double z, const double roll, const double pitch, + bool get_biased_yaw) const +{ + const double x = kalman_filter_.getXelement(IDX::X); + const double y = kalman_filter_.getXelement(IDX::Y); + const double biased_yaw = kalman_filter_.getXelement(IDX::YAW); + const double yaw_bias = kalman_filter_.getXelement(IDX::YAWB); + const double yaw = biased_yaw + yaw_bias; + Pose current_ekf_pose; + current_ekf_pose.header.frame_id = params_.pose_frame_id; + current_ekf_pose.header.stamp = current_time; + current_ekf_pose.pose.position = tier4_autoware_utils::createPoint(x, y, z); + if (get_biased_yaw) { + current_ekf_pose.pose.orientation = + tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, biased_yaw); + } else { + current_ekf_pose.pose.orientation = + tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, yaw); + } + return current_ekf_pose; +} + +geometry_msgs::msg::TwistStamped EKFModule::getCurrentTwist(const rclcpp::Time & current_time) const +{ + const double vx = kalman_filter_.getXelement(IDX::VX); + const double wz = kalman_filter_.getXelement(IDX::WZ); + + Twist current_ekf_twist; + current_ekf_twist.header.frame_id = "base_link"; + current_ekf_twist.header.stamp = current_time; + current_ekf_twist.twist.linear.x = vx; + current_ekf_twist.twist.angular.z = wz; + return current_ekf_twist; +} + +std::array EKFModule::getCurrentPoseCovariance() const +{ + return ekfCovarianceToPoseMessageCovariance(kalman_filter_.getLatestP()); +} + +std::array EKFModule::getCurrentTwistCovariance() const +{ + return ekfCovarianceToTwistMessageCovariance(kalman_filter_.getLatestP()); +} + +double EKFModule::getYawBias() const +{ + return kalman_filter_.getLatestX()(IDX::YAWB); +} + +void EKFModule::predictWithDelay(const double dt) +{ + const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX(); + const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP(); + + const double proc_cov_vx_d = std::pow(params_.proc_stddev_vx_c * dt, 2.0); + const double proc_cov_wz_d = std::pow(params_.proc_stddev_wz_c * dt, 2.0); + const double proc_cov_yaw_d = std::pow(params_.proc_stddev_yaw_c * dt, 2.0); + + const Vector6d X_next = predictNextState(X_curr, dt); + const Matrix6d A = createStateTransitionMatrix(X_curr, dt); + const Matrix6d Q = processNoiseCovariance(proc_cov_yaw_d, proc_cov_vx_d, proc_cov_wz_d); + kalman_filter_.predictWithDelay(X_next, A, Q); +} + +bool EKFModule::measurementUpdatePose( + const PoseWithCovariance & pose, const double dt, const rclcpp::Time & t_curr, + EKFDiagnosticInfo & pose_diag_info) +{ + if (pose.header.frame_id != params_.pose_frame_id) { + warning_->warnThrottle( + fmt::format( + "pose frame_id is %s, but pose_frame is set as %s. They must be same.", + pose.header.frame_id.c_str(), params_.pose_frame_id.c_str()), + 2000); + } + const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX(); + DEBUG_PRINT_MAT(X_curr.transpose()); + + constexpr int dim_y = 3; // pos_x, pos_y, yaw, depending on Pose output + + /* Calculate delay step */ + double delay_time = (t_curr - pose.header.stamp).seconds() + params_.pose_additional_delay; + if (delay_time < 0.0) { + warning_->warnThrottle(poseDelayTimeWarningMessage(delay_time), 1000); + } + + delay_time = std::max(delay_time, 0.0); + + const int delay_step = std::roundf(delay_time / dt); + + pose_diag_info.delay_time = std::max(delay_time, pose_diag_info.delay_time); + pose_diag_info.delay_time_threshold = params_.extend_state_step * dt; + if (delay_step >= params_.extend_state_step) { + pose_diag_info.is_passed_delay_gate = false; + warning_->warnThrottle( + poseDelayStepWarningMessage(delay_time, params_.extend_state_step, dt), 2000); + return false; + } + + /* Set yaw */ + double yaw = tf2::getYaw(pose.pose.pose.orientation); + const double ekf_yaw = kalman_filter_.getXelement(delay_step * dim_x_ + IDX::YAW); + const double yaw_error = normalizeYaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi + yaw = yaw_error + ekf_yaw; + + /* Set measurement matrix */ + Eigen::MatrixXd y(dim_y, 1); + y << pose.pose.pose.position.x, pose.pose.pose.position.y, yaw; + + if (hasNan(y) || hasInf(y)) { + warning_->warn( + "[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message."); + return false; + } + + /* Gate */ + const Eigen::Vector3d y_ekf( + kalman_filter_.getXelement(delay_step * dim_x_ + IDX::X), + kalman_filter_.getXelement(delay_step * dim_x_ + IDX::Y), ekf_yaw); + const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP(); + const Eigen::MatrixXd P_y = P_curr.block(0, 0, dim_y, dim_y); + + const double distance = mahalanobis(y_ekf, y, P_y); + pose_diag_info.mahalanobis_distance = std::max(distance, pose_diag_info.mahalanobis_distance); + if (distance > params_.pose_gate_dist) { + pose_diag_info.is_passed_mahalanobis_gate = false; + warning_->warnThrottle(mahalanobisWarningMessage(distance, params_.pose_gate_dist), 2000); + warning_->warnThrottle("Ignore the measurement data.", 2000); + return false; + } + + DEBUG_PRINT_MAT(y.transpose()); + DEBUG_PRINT_MAT(y_ekf.transpose()); + DEBUG_PRINT_MAT((y - y_ekf).transpose()); + + const Eigen::Matrix C = poseMeasurementMatrix(); + const Eigen::Matrix3d R = + poseMeasurementCovariance(pose.pose.covariance, params_.pose_smoothing_steps); + + kalman_filter_.updateWithDelay(y, C, R, delay_step); + + // debug + const Eigen::MatrixXd X_result = kalman_filter_.getLatestX(); + DEBUG_PRINT_MAT(X_result.transpose()); + DEBUG_PRINT_MAT((X_result - X_curr).transpose()); + + return true; +} + +geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensatePoseWithZDelay( + const PoseWithCovariance & pose, const double delay_time) +{ + const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation); + const double dz_delay = kalman_filter_.getXelement(IDX::VX) * delay_time * std::sin(-rpy.y); + PoseWithCovariance pose_with_z_delay; + pose_with_z_delay = pose; + pose_with_z_delay.pose.pose.position.z += dz_delay; + return pose_with_z_delay; +} + +bool EKFModule::measurementUpdateTwist( + const TwistWithCovariance & twist, const double dt, const rclcpp::Time & t_curr, + EKFDiagnosticInfo & twist_diag_info) +{ + if (twist.header.frame_id != "base_link") { + warning_->warnThrottle("twist frame_id must be base_link", 2000); + } + + const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX(); + DEBUG_PRINT_MAT(X_curr.transpose()); + + constexpr int dim_y = 2; // vx, wz + + /* Calculate delay step */ + double delay_time = (t_curr - twist.header.stamp).seconds() + params_.twist_additional_delay; + if (delay_time < 0.0) { + warning_->warnThrottle(twistDelayTimeWarningMessage(delay_time), 1000); + } + delay_time = std::max(delay_time, 0.0); + + const int delay_step = std::roundf(delay_time / dt); + + twist_diag_info.delay_time = std::max(delay_time, twist_diag_info.delay_time); + twist_diag_info.delay_time_threshold = params_.extend_state_step * dt; + if (delay_step >= params_.extend_state_step) { + twist_diag_info.is_passed_delay_gate = false; + warning_->warnThrottle( + twistDelayStepWarningMessage(delay_time, params_.extend_state_step, dt), 2000); + return false; + } + + /* Set measurement matrix */ + Eigen::MatrixXd y(dim_y, 1); + y << twist.twist.twist.linear.x, twist.twist.twist.angular.z; + + if (hasNan(y) || hasInf(y)) { + warning_->warn( + "[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message."); + return false; + } + + const Eigen::Vector2d y_ekf( + kalman_filter_.getXelement(delay_step * dim_x_ + IDX::VX), + kalman_filter_.getXelement(delay_step * dim_x_ + IDX::WZ)); + const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP(); + const Eigen::MatrixXd P_y = P_curr.block(4, 4, dim_y, dim_y); + + const double distance = mahalanobis(y_ekf, y, P_y); + twist_diag_info.mahalanobis_distance = std::max(distance, twist_diag_info.mahalanobis_distance); + if (distance > params_.twist_gate_dist) { + twist_diag_info.is_passed_mahalanobis_gate = false; + warning_->warnThrottle(mahalanobisWarningMessage(distance, params_.twist_gate_dist), 2000); + warning_->warnThrottle("Ignore the measurement data.", 2000); + return false; + } + + DEBUG_PRINT_MAT(y.transpose()); + DEBUG_PRINT_MAT(y_ekf.transpose()); + DEBUG_PRINT_MAT((y - y_ekf).transpose()); + + const Eigen::Matrix C = twistMeasurementMatrix(); + const Eigen::Matrix2d R = + twistMeasurementCovariance(twist.twist.covariance, params_.twist_smoothing_steps); + + kalman_filter_.updateWithDelay(y, C, R, delay_step); + + // debug + const Eigen::MatrixXd X_result = kalman_filter_.getLatestX(); + DEBUG_PRINT_MAT(X_result.transpose()); + DEBUG_PRINT_MAT((X_result - X_curr).transpose()); + + return true; +} diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index 97d4a02472f2d..3a6358e62c21a 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -15,6 +15,7 @@ #ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ #define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ +#include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/ros/msg_covariance.hpp" #include "tier4_autoware_utils/ros/transform_listener.hpp" @@ -63,6 +64,7 @@ class GyroOdometer : public rclcpp::Node twist_with_covariance_pub_; std::shared_ptr transform_listener_; + std::unique_ptr logger_configure_; std::string output_frame_; double message_timeout_sec_; diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index 2412fd5d57602..c14c48ffb2046 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -108,6 +108,7 @@ GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options) imu_arrived_(false) { transform_listener_ = std::make_shared(this); + logger_configure_ = std::make_unique(this); vehicle_twist_sub_ = create_subscription( "vehicle/twist_with_covariance", rclcpp::QoS{100}, diff --git a/localization/landmark_based_localizer/README.md b/localization/landmark_based_localizer/README.md index a8ebdc0c8f1e5..35d8d78e7015c 100644 --- a/localization/landmark_based_localizer/README.md +++ b/localization/landmark_based_localizer/README.md @@ -23,11 +23,11 @@ This calculated ego pose is passed to the EKF, where it is fused with the twist ![node diagram](./doc_image/node_diagram.drawio.svg) -### `landmark_tf_caster` node +### `landmark_parser` The definitions of the landmarks written to the map are introduced in the next section. See `Map Specifications`. -The `landmark_tf_caster` node publishes the TF from the map to the landmark. +The `landmark_parser` is a utility package to load landmarks from the map. - Translation : The center of the four vertices of the landmark - Rotation : Let the vertex numbers be 1, 2, 3, 4 counterclockwise as shown in the next section. Direction is defined as the cross product of the vector from 1 to 2 and the vector from 2 to 3. @@ -41,25 +41,9 @@ So, if the 4 vertices are considered as forming a tetrahedron and its volume exc - ar_tag_based_localizer - etc. -## Inputs / Outputs - -### `landmark_tf_caster` node - -#### Input - -| Name | Type | Description | -| :--------------------- | :------------------------------------------- | :--------------- | -| `~/input/lanelet2_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Data of lanelet2 | - -#### Output - -| Name | Type | Description | -| :---------- | :------------------------------------- | :----------------- | -| `tf_static` | `geometry_msgs::msg::TransformStamped` | TF from map to tag | - ## Map specifications -For this package to work correctly, the poses of the landmarks must be specified in the Lanelet2 map format that `map_loader` and `landmark_tf_caster` can interpret. +For this package to work correctly, the poses of the landmarks must be specified in the Lanelet2 map format that `map_loader` and `landmark_parser` can interpret. The four vertices of a landmark are defined counterclockwise. diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt b/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt index 15aecd1f8ded9..d625064b8f6cb 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt @@ -15,8 +15,8 @@ ament_auto_find_build_dependencies() find_package(OpenCV REQUIRED) ament_auto_add_executable(ar_tag_based_localizer - src/ar_tag_based_localizer_node.cpp - src/ar_tag_based_localizer_core.cpp + src/main.cpp + src/ar_tag_based_localizer.cpp ) target_include_directories(ar_tag_based_localizer SYSTEM PUBLIC @@ -24,6 +24,19 @@ target_include_directories(ar_tag_based_localizer ) target_link_libraries(ar_tag_based_localizer ${OpenCV_LIBRARIES}) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_ar_tag_based_localizer + test/test.cpp + src/ar_tag_based_localizer.cpp + ) + target_include_directories(test_ar_tag_based_localizer + SYSTEM PUBLIC + ${OpenCV_INCLUDE_DIRS} + ) + target_link_libraries(test_ar_tag_based_localizer ${OpenCV_LIBRARIES}) +endif() + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md index b212711f38a57..f0c54d6393f7e 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md @@ -13,11 +13,12 @@ The positions and orientations of the AR-Tags are assumed to be written in the L #### Input -| Name | Type | Description | -| :-------------------- | :---------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -| `~/input/image` | `sensor_msgs::msg::Image` | Camera Image | -| `~/input/camera_info` | `sensor_msgs::msg::CameraInfo` | Camera Info | -| `~/input/ekf_pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose without IMU correction. It is used to validate detected AR tags by filtering out False Positives. Only if the EKF Pose and the AR tag-detected Pose are within a certain temporal and spatial range, the AR tag-detected Pose is considered valid and published. | +| Name | Type | Description | +| :--------------------- | :---------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `~/input/lanelet2_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Data of lanelet2 | +| `~/input/image` | `sensor_msgs::msg::Image` | Camera Image | +| `~/input/camera_info` | `sensor_msgs::msg::CameraInfo` | Camera Info | +| `~/input/ekf_pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose without IMU correction. It is used to validate detected AR tags by filtering out False Positives. Only if the EKF Pose and the AR tag-detected Pose are within a certain temporal and spatial range, the AR tag-detected Pose is considered valid and published. | #### Output @@ -25,6 +26,7 @@ The positions and orientations of the AR-Tags are assumed to be written in the L | :------------------------------ | :---------------------------------------------- | :---------------------------------------------------------------------------------------- | | `~/output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated Pose | | `~/debug/result` | `sensor_msgs::msg::Image` | [debug topic] Image in which marker detection results are superimposed on the input image | +| `~/debug/marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] Loaded landmarks to visualize in Rviz as thin boards | | `/tf` | `geometry_msgs::msg::TransformStamped` | [debug topic] TF from camera to detected tag | | `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostics outputs | diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml index f6c10050b1826..6437455875cc2 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml @@ -3,6 +3,7 @@ + @@ -11,6 +12,7 @@ + diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml index 216fa21bc951a..c0d1b00e35d3b 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml @@ -6,6 +6,8 @@ The ar_tag_based_localizer package Shintaro Sakoda Masahiro Sakamoto + Yamato Ando + Kento Yabuuchi Apache License 2.0 ament_cmake @@ -15,6 +17,8 @@ cv_bridge diagnostic_msgs image_transport + landmark_parser + lanelet2_extension rclcpp tf2_eigen tf2_geometry_msgs diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp similarity index 89% rename from localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp rename to localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index afa82ab3e0677..73e7b8b7e3587 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -42,7 +42,7 @@ or implied, of Rafael Muñoz Salinas. ********************************/ -#include "ar_tag_based_localizer/ar_tag_based_localizer_core.hpp" +#include "ar_tag_based_localizer.hpp" #include #include @@ -51,14 +51,15 @@ #include #include +#include #ifdef ROS_DISTRO_GALACTIC #include #else #include #endif -ArTagBasedLocalizer::ArTagBasedLocalizer() -: Node("ar_tag_based_localizer"), cam_info_received_(false) +ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) +: Node("ar_tag_based_localizer", options), cam_info_received_(false) { } @@ -110,6 +111,9 @@ bool ArTagBasedLocalizer::setup() /* Subscribers */ + map_bin_sub_ = this->create_subscription( + "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal), + std::bind(&ArTagBasedLocalizer::map_bin_callback, this, std::placeholders::_1)); rclcpp::QoS qos_sub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); qos_sub.best_effort(); image_sub_ = this->create_subscription( @@ -125,6 +129,11 @@ bool ArTagBasedLocalizer::setup() /* Publishers */ + rclcpp::QoS qos_marker = rclcpp::QoS(rclcpp::KeepLast(10)); + qos_marker.transient_local(); + qos_marker.reliable(); + marker_pub_ = + this->create_publisher("~/debug/marker", qos_marker); rclcpp::QoS qos_pub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); image_pub_ = it_->advertise("~/debug/result", 1); pose_pub_ = this->create_publisher( @@ -136,6 +145,15 @@ bool ArTagBasedLocalizer::setup() return true; } +void ArTagBasedLocalizer::map_bin_callback( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg) +{ + landmark_map_ = parse_landmark(msg, "apriltag_16h5", this->get_logger()); + const visualization_msgs::msg::MarkerArray marker_msg = + convert_to_marker_array_msg(landmark_map_); + marker_pub_->publish(marker_msg); +} + void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & msg) { if ((image_pub_.getNumSubscribers() == 0) && (pose_pub_->get_subscription_count() == 0)) { @@ -202,7 +220,7 @@ void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSha image_pub_.publish(out_msg.toImageMsg()); } - const int detected_tags = markers.size(); + const int detected_tags = static_cast(markers.size()); diagnostic_msgs::msg::DiagnosticStatus diag_status; @@ -290,16 +308,20 @@ void ArTagBasedLocalizer::publish_pose_as_base_link( } // Transform map to tag - geometry_msgs::msg::TransformStamped map_to_tag_tf; - try { - map_to_tag_tf = - tf_buffer_->lookupTransform("map", "tag_" + msg.header.frame_id, tf2::TimePointZero); - } catch (tf2::TransformException & ex) { - RCLCPP_INFO( - this->get_logger(), "Could not transform map to tag_%s: %s", msg.header.frame_id.c_str(), - ex.what()); + if (landmark_map_.count(msg.header.frame_id) == 0) { + RCLCPP_INFO_STREAM( + this->get_logger(), "frame_id(" << msg.header.frame_id << ") is not in landmark_map_"); return; } + const geometry_msgs::msg::Pose & tag_pose = landmark_map_.at(msg.header.frame_id); + geometry_msgs::msg::TransformStamped map_to_tag_tf; + map_to_tag_tf.header.stamp = msg.header.stamp; + map_to_tag_tf.header.frame_id = "map"; + map_to_tag_tf.child_frame_id = msg.header.frame_id; + map_to_tag_tf.transform.translation.x = tag_pose.position.x; + map_to_tag_tf.transform.translation.y = tag_pose.position.y; + map_to_tag_tf.transform.translation.z = tag_pose.position.z; + map_to_tag_tf.transform.rotation = tag_pose.orientation; // Transform camera to base_link geometry_msgs::msg::TransformStamped camera_to_base_link_tf; diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp similarity index 87% rename from localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp rename to localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp index 37725dd06c34e..6bd66eead653f 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp @@ -42,8 +42,10 @@ or implied, of Rafael Muñoz Salinas. ********************************/ -#ifndef AR_TAG_BASED_LOCALIZER__AR_TAG_BASED_LOCALIZER_CORE_HPP_ -#define AR_TAG_BASED_LOCALIZER__AR_TAG_BASED_LOCALIZER_CORE_HPP_ +#ifndef AR_TAG_BASED_LOCALIZER_HPP_ +#define AR_TAG_BASED_LOCALIZER_HPP_ + +#include "landmark_parser/landmark_parser_core.hpp" #include #include @@ -57,6 +59,7 @@ #include #include +#include #include #include #include @@ -64,10 +67,11 @@ class ArTagBasedLocalizer : public rclcpp::Node { public: - ArTagBasedLocalizer(); + explicit ArTagBasedLocalizer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); bool setup(); private: + void map_bin_callback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg); void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & msg); void cam_info_callback(const sensor_msgs::msg::CameraInfo & msg); void ekf_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped & msg); @@ -92,11 +96,13 @@ class ArTagBasedLocalizer : public rclcpp::Node std::unique_ptr it_; // Subscribers + rclcpp::Subscription::SharedPtr map_bin_sub_; rclcpp::Subscription::SharedPtr image_sub_; rclcpp::Subscription::SharedPtr cam_info_sub_; rclcpp::Subscription::SharedPtr ekf_pose_sub_; // Publishers + rclcpp::Publisher::SharedPtr marker_pub_; image_transport::Publisher image_pub_; rclcpp::Publisher::SharedPtr pose_pub_; rclcpp::Publisher::SharedPtr diag_pub_; @@ -106,6 +112,7 @@ class ArTagBasedLocalizer : public rclcpp::Node aruco::CameraParameters cam_param_; bool cam_info_received_; geometry_msgs::msg::PoseWithCovarianceStamped latest_ekf_pose_{}; + std::map landmark_map_; }; -#endif // AR_TAG_BASED_LOCALIZER__AR_TAG_BASED_LOCALIZER_CORE_HPP_ +#endif // AR_TAG_BASED_LOCALIZER_HPP_ diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp similarity index 97% rename from localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp rename to localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp index 92d2e35ee3c1b..cbcd57b4b222a 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp @@ -42,7 +42,7 @@ or implied, of Rafael Muñoz Salinas. ********************************/ -#include "ar_tag_based_localizer/ar_tag_based_localizer_core.hpp" +#include "ar_tag_based_localizer.hpp" int main(int argc, char ** argv) { 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 new file mode 100644 index 0000000000000..6b302ee6bc862 --- /dev/null +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp @@ -0,0 +1,67 @@ +// Copyright 2023 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 "../src/ar_tag_based_localizer.hpp" + +#include + +#include +#include + +#include +#include +#include + +class TestArTagBasedLocalizer : public ::testing::Test +{ +protected: + void SetUp() override + { + const std::string yaml_path = + "../../install/ar_tag_based_localizer/share/ar_tag_based_localizer/config/" + "ar_tag_based_localizer.param.yaml"; + + rcl_params_t * params_st = rcl_yaml_node_struct_init(rcl_get_default_allocator()); + if (!rcl_parse_yaml_file(yaml_path.c_str(), params_st)) { + std::cout << "Failed to parse yaml file" << std::endl; + return; + } + + const rclcpp::ParameterMap param_map = rclcpp::parameter_map_from(params_st, ""); + rclcpp::NodeOptions node_options; + for (const auto & param_pair : param_map) { + for (const auto & param : param_pair.second) { + node_options.parameter_overrides().push_back(param); + } + } + node_ = std::make_shared(node_options); + rcl_yaml_node_struct_fini(params_st); + } + + std::shared_ptr node_; +}; + +TEST_F(TestArTagBasedLocalizer, test_setup) // NOLINT +{ + EXPECT_TRUE(node_->setup()); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg b/localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg index 120a1f2aa3391..e592f06a5a86c 100644 --- a/localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg +++ b/localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg @@ -3,90 +3,84 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="721px" + width="568px" height="331px" - viewBox="-0.5 -0.5 721 331" - content="<mxfile><diagram id="z_GJx55S16-dNHi1224P" name="ページ1">1VnLjpswFP2aLBsFG0iybF7TxVQdaSq1XSEHDFgDGBnn1a+vHQwE8ExIQiYqm+BjA/Y51/fhDOA83j8xlIbfqYejARh5+wFcDAAwgA3Ej0QOOWIDMwcCRjw1qAJeyV+swJFCN8TDWW0gpzTiJK2DLk0S7PIahhiju/own0b1r6YowC3g1UVRG/1FPB7m6ASMK/wbJkFYfNmwp3lPjIrBaiVZiDy6O4HgcgDnjFKe38X7OY4keQUv+XOrd3rLiTGc8C4PKCG2KNqotal58UOxWEY3iYfl+NEAznyacCWIAVR7TiPKjoPh6HgJvD0RNbctZhzvTyA1sSdMY8zZQQxRvcBSJCkrgaq5qygvsfCEblthSKkclG+uiBA3igs9L2aLl2eUeDFibwKdoUzwAWyxXkOu9ZkK0xCcsHPckSg64Wo+X1orMZ9Zxhl9wyc9tjvBa/++7MJxnV2zza4x0rBr9sCu1WL3Bw8lfaOvG053Ylkn7KbIfRMbKruU3NVqPteROwFraNt3Nl37ceTCsWZPr/iOZNzZER46Lt0iRlDi4hal2BN+TjUp4yENaIKiZYXOBCfs8Ftxfmz8kY3h2Crai/1p7+KgWu+yyhELsIKgwuQ8PuSZ4Qhxsq37Yx1r6tEXSsRnK+Nv6APNSf0VGd0wF6unGtyX0+gkR+Hp63KQWIaYj+hPaPIR36AXvo3x5/BtQqvGNxg3DD2fVIvv1ovAqC4cgPBuwk10wqU0wxdto0LHPeFHGYeWakkdDXVfaSgbh1NBe9U+Z6eu/VX7T8nxZTQ0xJU/dKuNjOs2AuGlNlIMpL6f4VvlN7RuFDGHo8BZywzAiW6M++8FoHY+cLz6CU1mcwcBTWyyNLHJ6iM2GTpS8Zt/PZcqzD+ES7uZoRqfyaXWP7koxgw5JPHpLeGln3D+WeGlFRWsbuFcE6fqLzImVicfdIV/MaFGvvOCdXLtcNrWwuw9tepsqNPWSkUxhSPM2ytGWZoX7D7Zyz0/SzEj4otY7l7xWlHh45cKmgmfESSiyxV0HIGyoJZG6qEsLD1HJgoJkgQ/aSoAKIA8Byt/FyQOxFIispZ+3pUUOB5hYjpUrnflIY6k38+G2Tbox39MGtYGZHbQdCA6/2FOb5fFbFdhsk7wnYwL/d0uxljya3S1zKLAua+X6EyBNjePVKXvCC5clPH/q65vxnfDmGqM6m5RyWyfnPTm1kxNiCkkfIDxaHOZSHk2J0apSGqQd3VG85iDi6b1aF3S/azH1nEq88OL664zxxdGPd85l+7oy7eyYtOWb91C9QPTpuYRILw2bbKbxynWeNhX4iSa1XF4Prz6UwEu/wE=</diagram></mxfile>" + viewBox="-0.5 -0.5 568 331" + content="<mxfile><diagram id="z_GJx55S16-dNHi1224P" name="ページ1">xZhLj5swEMc/TY6NMOaRHJvX9rBVV2qltqfIAQesAEbGefXT1w4GAng3JCGbHHbxH2Ps34xnxgzgND68MJSG36mPo4Fp+IcBnA1M0wVA/JXCMRegO8qFgBE/l0Al/CT/sBINpW6Jj7NaR05pxElaFz2aJNjjNQ0xRvf1bmsa1d+aogC3hJ8eitrqb+LzMFdHplvp3zAJwuLNwBnnd2JUdFYryULk0/2ZBOcDOGWU8vwqPkxxJNkVXPLnFu/cLSfGcMK7PGDmD+xQtFVrU/Pix2KxjG4TH8v+xgBO1jThyiDAVO0pjSg7dYbG6Sf09kTU3HaYcXw4k9TEXjCNMWdH0UXdNW0FqfAS1dxXyEstPMPtKA0pKwflyBUIcaFY6LlYLS6vKPFjxDZCnaBM8DAdsV4g1/pKhWsIJuwSOxJFZ6ym07m9EPOZZJzRDT6743gjvFo/li5063StNl1gaOhaPdC1W3R/8FDiM75uOd2LZZ3RTZG3ERsquxbuYjGd6uCOzBV0nAe7rvM8uNDV7OkF35OML/eEh0uP7hAjKPFwCyn2RZxTTcp4SAOaoGheqRPBhB3/KOanxl/ZGLp20Z4dzu/Ojqr1LlWOWICVBJUm5/EhZ4YjxMmuHo911NSjb5SI11bO37APtEb1ITK6ZR5WTzXYl9PoZI4i0tfNQWKZYj7Cn9DkI95mL7yB+zm8LWjXeJtuw9HzSbV4twYqd0gxEIQPM9xIZ7iUZviqbVTY8UD4yYxDW7WkHYG6rmwoG8dzg/Zq+5xO3fY37T9lji/GEICimrvXR9y6j0B4rY8UHel6neF7zQ+0YRSxJUfBciUrgGV0Z95/LwG164HTr5/UZBmNHWRqcpOtyU12H7kJ6KDizfp2lirNP4Wl06xQwWey1MYnD8WYoSVJ1vSe9NJPOv+s9NLKCna3dK7JU/WBwMjuFINuiC8W1JjvssE6hXY4btvC6r206uyo49ZKxWEKR5i3V4yyND+wr8lB7vlJihkRb8Ry94phxQkfv1XSRMSMIBG3PIHjJJQHaumkPsrCMnJk4iBBkuAXTYUAhZDXYOX/GYkDsZSIrGSc9ySCpU+YmA6V6134iCMZ97Nhtgt6Oia447q7mbI8aEYQXQCxxvfbxWofw14pEgdbg56KGgPxk3EQ49u0i2+WuEFXR7WeWZM26g3gjIeN3d5fMWlpE1+ktsEyRqnIgMi/Of095ZQLx13c92EJ0HJ0TGUxcXWRfuGsC+rJ8VJu1Nf6ZXmvrfW7xfUnbpfm9yJ4a451mmdv223uu5uzrGhW307z7tUHaDj/Dw==</diagram></mxfile>" > - - + + - Landmark Based - Localizer + Landmark Based + Localizer - + - Other Autoware - packages + Other Autoware + packages - - + + - - /twist_with_covariance + + /twist_with_covariance - - + + - - /image + + /image - - + + - - /pose_with_covariance + + /pose_with_covariance - + - /ar_tag_based_localizer + /ar_tag_based_localizer - + - /ekf_localizer + /ekf_localizer - - + + - - /camera_info + + /camera_info - - + + - - Lanelet2 + + Lanelet2 - - + + - - /tf_static + + Load once at startup - + - /landmark_tf_caster + /lanelet2_map_loader - - - - - /lanelet2_map_loader - - - + + - - /ekf_pose_with_covariance + + /ekf_pose_with_covariance diff --git a/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt b/localization/landmark_based_localizer/landmark_parser/CMakeLists.txt similarity index 69% rename from localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt rename to localization/landmark_based_localizer/landmark_parser/CMakeLists.txt index 8d8cb546b6162..41f7c00d61383 100644 --- a/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt +++ b/localization/landmark_based_localizer/landmark_parser/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(landmark_tf_caster) +project(landmark_parser) find_package(autoware_cmake REQUIRED) autoware_package() @@ -12,13 +12,10 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -ament_auto_add_executable(landmark_tf_caster - src/landmark_tf_caster_node.cpp - src/landmark_tf_caster_core.cpp +ament_auto_add_library(landmark_parser + src/landmark_parser_core.cpp ) ament_auto_package( INSTALL_TO_SHARE - launch - config ) diff --git a/localization/landmark_based_localizer/landmark_parser/include/landmark_parser/landmark_parser_core.hpp b/localization/landmark_based_localizer/landmark_parser/include/landmark_parser/landmark_parser_core.hpp new file mode 100644 index 0000000000000..edf45c2a2997a --- /dev/null +++ b/localization/landmark_based_localizer/landmark_parser/include/landmark_parser/landmark_parser_core.hpp @@ -0,0 +1,34 @@ +// Copyright 2023 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. + +#ifndef LANDMARK_PARSER__LANDMARK_PARSER_CORE_HPP_ +#define LANDMARK_PARSER__LANDMARK_PARSER_CORE_HPP_ + +#include + +#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" +#include +#include + +#include +#include + +std::map parse_landmark( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, + const std::string & target_subtype, const rclcpp::Logger & logger); + +visualization_msgs::msg::MarkerArray convert_to_marker_array_msg( + const std::map & landmarks); + +#endif // LANDMARK_PARSER__LANDMARK_PARSER_CORE_HPP_ diff --git a/localization/landmark_based_localizer/landmark_tf_caster/package.xml b/localization/landmark_based_localizer/landmark_parser/package.xml similarity index 69% rename from localization/landmark_based_localizer/landmark_tf_caster/package.xml rename to localization/landmark_based_localizer/landmark_parser/package.xml index 75f42b91f502a..e3daa93f81220 100644 --- a/localization/landmark_based_localizer/landmark_tf_caster/package.xml +++ b/localization/landmark_based_localizer/landmark_parser/package.xml @@ -1,22 +1,24 @@ - landmark_tf_caster + landmark_parser 0.0.0 - The landmark_tf_caster package + The landmark_parser package Shintaro Sakoda Masahiro Sakamoto + Yamato Ando + Kento Yabuuchi Apache License 2.0 ament_cmake autoware_cmake + eigen + autoware_auto_mapping_msgs + geometry_msgs lanelet2_extension rclcpp - tf2_eigen - tf2_geometry_msgs - tf2_ros ament_cmake diff --git a/localization/landmark_based_localizer/landmark_parser/src/landmark_parser_core.cpp b/localization/landmark_based_localizer/landmark_parser/src/landmark_parser_core.cpp new file mode 100644 index 0000000000000..c86b35b6115b4 --- /dev/null +++ b/localization/landmark_based_localizer/landmark_parser/src/landmark_parser_core.cpp @@ -0,0 +1,154 @@ +// Copyright 2023 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 "landmark_parser/landmark_parser_core.hpp" + +#include "lanelet2_extension/utility/message_conversion.hpp" + +#include + +#include +#include + +std::map parse_landmark( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, + const std::string & target_subtype, const rclcpp::Logger & logger) +{ + RCLCPP_INFO_STREAM(logger, "msg->format_version: " << msg->format_version); + RCLCPP_INFO_STREAM(logger, "msg->map_format: " << msg->map_format); + RCLCPP_INFO_STREAM(logger, "msg->map_version: " << msg->map_version); + RCLCPP_INFO_STREAM(logger, "msg->data.size(): " << msg->data.size()); + lanelet::LaneletMapPtr lanelet_map_ptr{std::make_shared()}; + lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr); + + std::map landmark_map; + + for (const auto & poly : lanelet_map_ptr->polygonLayer) { + const std::string type{poly.attributeOr(lanelet::AttributeName::Type, "none")}; + if (type != "pose_marker") { + continue; + } + const std::string subtype{poly.attributeOr(lanelet::AttributeName::Subtype, "none")}; + if (subtype != target_subtype) { + continue; + } + + // Get marker_id + const std::string marker_id = poly.attributeOr("marker_id", "none"); + + // Get 4 vertices + const auto & vertices = poly.basicPolygon(); + if (vertices.size() != 4) { + RCLCPP_WARN_STREAM(logger, "vertices.size() (" << vertices.size() << ") != 4"); + continue; + } + + // 4 vertices represent the marker vertices in counterclockwise order + // Calculate the volume by considering it as a tetrahedron + const auto & v0 = vertices[0]; + const auto & v1 = vertices[1]; + const auto & v2 = vertices[2]; + const auto & v3 = vertices[3]; + const double volume = (v1 - v0).cross(v2 - v0).dot(v3 - v0) / 6.0; + RCLCPP_INFO_STREAM(logger, "volume = " << volume); + const double volume_threshold = 1e-5; + if (volume > volume_threshold) { + RCLCPP_WARN_STREAM( + logger, + "volume (" << volume << ") > threshold (" << volume_threshold << "), This is not plane."); + continue; + } + + // Calculate the center of the quadrilateral + const auto center = (v0 + v1 + v2 + v3) / 4.0; + + // Define axes + const auto x_axis = (v1 - v0).normalized(); + const auto y_axis = (v2 - v1).normalized(); + const auto z_axis = x_axis.cross(y_axis).normalized(); + + // Construct rotation matrix and convert it to quaternion + Eigen::Matrix3d rotation_matrix; + rotation_matrix << x_axis, y_axis, z_axis; + const Eigen::Quaterniond q{rotation_matrix}; + + // Create Pose + geometry_msgs::msg::Pose pose; + pose.position.x = center.x(); + pose.position.y = center.y(); + pose.position.z = center.z(); + pose.orientation.x = q.x(); + pose.orientation.y = q.y(); + pose.orientation.z = q.z(); + pose.orientation.w = q.w(); + + // Add to map + landmark_map[marker_id] = pose; + RCLCPP_INFO_STREAM(logger, "id: " << marker_id); + RCLCPP_INFO_STREAM( + logger, + "(x, y, z) = " << pose.position.x << ", " << pose.position.y << ", " << pose.position.z); + RCLCPP_INFO_STREAM( + logger, "q = " << pose.orientation.x << ", " << pose.orientation.y << ", " + << pose.orientation.z << ", " << pose.orientation.w); + } + + return landmark_map; +} + +visualization_msgs::msg::MarkerArray convert_to_marker_array_msg( + const std::map & landmarks) +{ + int32_t id = 0; + visualization_msgs::msg::MarkerArray marker_array; + for (const auto & [id_str, pose] : landmarks) { + // publish cube as a thin board + visualization_msgs::msg::Marker cube_marker; + cube_marker.header.frame_id = "map"; + cube_marker.header.stamp = rclcpp::Clock().now(); + cube_marker.ns = "landmark_cube"; + cube_marker.id = id; + cube_marker.type = visualization_msgs::msg::Marker::CUBE; + cube_marker.action = visualization_msgs::msg::Marker::ADD; + cube_marker.pose = pose; + cube_marker.scale.x = 1.0; + cube_marker.scale.y = 2.0; + cube_marker.scale.z = 0.1; + cube_marker.color.a = 0.5; + cube_marker.color.r = 0.0; + cube_marker.color.g = 1.0; + cube_marker.color.b = 0.0; + marker_array.markers.push_back(cube_marker); + + // publish text + visualization_msgs::msg::Marker text_marker; + text_marker.header.frame_id = "map"; + text_marker.header.stamp = rclcpp::Clock().now(); + text_marker.ns = "landmark_text"; + text_marker.id = id; + text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + text_marker.action = visualization_msgs::msg::Marker::ADD; + text_marker.pose = pose; + text_marker.text = "(" + id_str + ")"; + text_marker.scale.z = 0.5; + text_marker.color.a = 1.0; + text_marker.color.r = 1.0; + text_marker.color.g = 0.0; + text_marker.color.b = 0.0; + marker_array.markers.push_back(text_marker); + + id++; + } + return marker_array; +} diff --git a/localization/landmark_based_localizer/landmark_tf_caster/config/landmark_tf_caster.param.yaml b/localization/landmark_based_localizer/landmark_tf_caster/config/landmark_tf_caster.param.yaml deleted file mode 100644 index 2fe2869038e82..0000000000000 --- a/localization/landmark_based_localizer/landmark_tf_caster/config/landmark_tf_caster.param.yaml +++ /dev/null @@ -1,4 +0,0 @@ -/**: - ros__parameters: - # Tags with a volume greater than this value will not cast their pose - volume_threshold: 1e-5 # [m^3] diff --git a/localization/landmark_based_localizer/landmark_tf_caster/include/landmark_tf_caster/landmark_tf_caster_core.hpp b/localization/landmark_based_localizer/landmark_tf_caster/include/landmark_tf_caster/landmark_tf_caster_core.hpp deleted file mode 100644 index 0eb6706a0b5b0..0000000000000 --- a/localization/landmark_based_localizer/landmark_tf_caster/include/landmark_tf_caster/landmark_tf_caster_core.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2023 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. - -#ifndef LANDMARK_TF_CASTER__LANDMARK_TF_CASTER_CORE_HPP_ -#define LANDMARK_TF_CASTER__LANDMARK_TF_CASTER_CORE_HPP_ - -#include - -#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" -#include - -#include -#include -#include - -#include - -class LandmarkTfCaster : public rclcpp::Node -{ -public: - LandmarkTfCaster(); - -private: - void map_bin_callback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg); - void publish_tf(const lanelet::Polygon3d & poly); - - // Parameters - double volume_threshold_; - - // tf - std::unique_ptr tf_buffer_; - std::unique_ptr tf_broadcaster_; - - // Subscribers - rclcpp::Subscription::SharedPtr map_bin_sub_; -}; - -#endif // LANDMARK_TF_CASTER__LANDMARK_TF_CASTER_CORE_HPP_ diff --git a/localization/landmark_based_localizer/landmark_tf_caster/launch/landmark_tf_caster.launch.xml b/localization/landmark_based_localizer/landmark_tf_caster/launch/landmark_tf_caster.launch.xml deleted file mode 100644 index eeaba555cb6f5..0000000000000 --- a/localization/landmark_based_localizer/landmark_tf_caster/launch/landmark_tf_caster.launch.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_core.cpp b/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_core.cpp deleted file mode 100644 index 801e036415326..0000000000000 --- a/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_core.cpp +++ /dev/null @@ -1,126 +0,0 @@ -// Copyright 2023 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 "landmark_tf_caster/landmark_tf_caster_core.hpp" - -#include "lanelet2_extension/utility/message_conversion.hpp" - -#include -#include - -#include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -LandmarkTfCaster::LandmarkTfCaster() : Node("landmark_tf_caster") -{ - // Parameters - volume_threshold_ = this->declare_parameter("volume_threshold", 1e-5); - - // tf - tf_buffer_ = std::make_unique(this->get_clock()); - tf_broadcaster_ = std::make_unique(this); - - // Subscribers - map_bin_sub_ = this->create_subscription( - "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal), - std::bind(&LandmarkTfCaster::map_bin_callback, this, std::placeholders::_1)); - RCLCPP_INFO(this->get_logger(), "finish setup"); -} - -void LandmarkTfCaster::map_bin_callback( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg) -{ - RCLCPP_INFO_STREAM(this->get_logger(), "msg->format_version: " << msg->format_version); - RCLCPP_INFO_STREAM(this->get_logger(), "msg->map_format: " << msg->map_format); - RCLCPP_INFO_STREAM(this->get_logger(), "msg->map_version: " << msg->map_version); - RCLCPP_INFO_STREAM(this->get_logger(), "msg->data.size(): " << msg->data.size()); - lanelet::LaneletMapPtr lanelet_map_ptr{std::make_shared()}; - lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr); - for (const auto & poly : lanelet_map_ptr->polygonLayer) { - const std::string type{poly.attributeOr(lanelet::AttributeName::Type, "none")}; - if (type != "pose_marker") { - continue; - } - publish_tf(poly); - } -} - -void LandmarkTfCaster::publish_tf(const lanelet::Polygon3d & poly) -{ - // Get marker_id - const std::string marker_id = poly.attributeOr("marker_id", "none"); - - // Get 4 vertices - const auto & vertices = poly.basicPolygon(); - if (vertices.size() != 4) { - RCLCPP_WARN_STREAM(this->get_logger(), "vertices.size() (" << vertices.size() << ") != 4"); - return; - } - - // 4 vertices represent the marker vertices in counterclockwise order - // Calculate the volume by considering it as a tetrahedron - const auto & v0 = vertices[0]; - const auto & v1 = vertices[1]; - const auto & v2 = vertices[2]; - const auto & v3 = vertices[3]; - const double volume = (v1 - v0).cross(v2 - v0).dot(v3 - v0) / 6.0; - RCLCPP_INFO_STREAM(this->get_logger(), "volume = " << volume); - if (volume > volume_threshold_) { - RCLCPP_WARN_STREAM( - this->get_logger(), - "volume (" << volume << ") > threshold (" << volume_threshold_ << "), This is not plane."); - return; - } - - // Calculate the center of the quadrilateral - const auto center = (v0 + v1 + v2 + v3) / 4.0; - - // Define axes - const auto x_axis = (v1 - v0).normalized(); - const auto y_axis = (v2 - v1).normalized(); - const auto z_axis = x_axis.cross(y_axis).normalized(); - - // Construct rotation matrix and convert it to quaternion - Eigen::Matrix3d rotation_matrix; - rotation_matrix << x_axis, y_axis, z_axis; - const Eigen::Quaterniond q{rotation_matrix}; - - // Create transform - geometry_msgs::msg::TransformStamped tf; - tf.header.stamp = this->now(); - tf.header.frame_id = "map"; - tf.child_frame_id = "tag_" + marker_id; - tf.transform.translation.x = center.x(); - tf.transform.translation.y = center.y(); - tf.transform.translation.z = center.z(); - tf.transform.rotation.x = q.x(); - tf.transform.rotation.y = q.y(); - tf.transform.rotation.z = q.z(); - tf.transform.rotation.w = q.w(); - - // Publish transform - tf_broadcaster_->sendTransform(tf); - RCLCPP_INFO_STREAM(this->get_logger(), "id: " << marker_id); - RCLCPP_INFO_STREAM( - this->get_logger(), "(x, y, z) = " << tf.transform.translation.x << ", " - << tf.transform.translation.y << ", " - << tf.transform.translation.z); - RCLCPP_INFO_STREAM( - this->get_logger(), "q = " << tf.transform.rotation.x << ", " << tf.transform.rotation.y << ", " - << tf.transform.rotation.z << ", " << tf.transform.rotation.w); -} diff --git a/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_node.cpp b/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_node.cpp deleted file mode 100644 index 4c34e593e7552..0000000000000 --- a/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_node.cpp +++ /dev/null @@ -1,22 +0,0 @@ -// Copyright 2023 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 "landmark_tf_caster/landmark_tf_caster_core.hpp" - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); -} diff --git a/localization/localization_error_monitor/include/localization_error_monitor/node.hpp b/localization/localization_error_monitor/include/localization_error_monitor/node.hpp index 7bdc87d5d47f2..296b278004333 100644 --- a/localization/localization_error_monitor/include/localization_error_monitor/node.hpp +++ b/localization/localization_error_monitor/include/localization_error_monitor/node.hpp @@ -17,11 +17,14 @@ #include #include +#include #include #include #include +#include + struct Ellipse { double long_radius; @@ -39,6 +42,9 @@ class LocalizationErrorMonitor : public rclcpp::Node rclcpp::Publisher::SharedPtr diag_pub_; rclcpp::TimerBase::SharedPtr timer_; + + std::unique_ptr logger_configure_; + double scale_; double error_ellipse_size_; double warn_ellipse_size_; diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index 4cc5599fd81a9..266ae6c848834 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -20,6 +20,7 @@ nav_msgs tf2 tf2_geometry_msgs + tier4_autoware_utils visualization_msgs ament_cmake_ros diff --git a/localization/localization_error_monitor/src/node.cpp b/localization/localization_error_monitor/src/node.cpp index 43795d4036d42..f47372a0158b2 100644 --- a/localization/localization_error_monitor/src/node.cpp +++ b/localization/localization_error_monitor/src/node.cpp @@ -53,6 +53,8 @@ LocalizationErrorMonitor::LocalizationErrorMonitor() : Node("localization_error_ this->create_publisher("debug/ellipse_marker", durable_qos); diag_pub_ = this->create_publisher("/diagnostics", 10); + + logger_configure_ = std::make_unique(this); } visualization_msgs::msg::Marker LocalizationErrorMonitor::createEllipseMarker( diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 79491a85a0a66..6fe61cd25bc6e 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -22,6 +22,7 @@ else() endif() endif() +find_package(glog REQUIRED) find_package(PCL REQUIRED COMPONENTS common io registration) include_directories(${PCL_INCLUDE_DIRS}) @@ -34,7 +35,7 @@ ament_auto_add_executable(ndt_scan_matcher ) link_directories(${PCL_LIBRARY_DIRS}) -target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES}) +target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES} glog::glog) if(BUILD_TESTING) add_ros_test( diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index e210d3a28796f..d7a7b1c5c37f3 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -34,6 +34,8 @@ One optional function is regularization. Please see the regularization chapter i | `points_aligned` | `sensor_msgs::msg::PointCloud2` | [debug topic] pointcloud aligned by scan matching | | `points_aligned_no_ground` | `sensor_msgs::msg::PointCloud2` | [debug topic] de-grounded pointcloud aligned by scan matching | | `initial_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] initial pose used in scan matching | +| `multi_ndt_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] estimated poses from multiple initial poses in real-time covariance estimation | +| `multi_initial_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] initial poses for real-time covariance estimation | | `exe_time_ms` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] execution time for scan matching [ms] | | `transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching | | `no_ground_transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on de-grounded LiDAR scan | @@ -257,3 +259,26 @@ This is a function that uses de-grounded LiDAR scan to estimate the scan matchin | ------------------------------------- | ------ | ------------------------------------------------------------------------------------- | | `estimate_scores_for_degrounded_scan` | bool | Flag for using scan matching score based on de-grounded LiDAR scan (FALSE by default) | | `z_margin_for_ground_removal` | double | Z-value margin for removal ground points | + +## 2D real-time covariance estimation + +### Abstract + +Calculate 2D covariance (xx, xy, yx, yy) in real time using the NDT convergence from multiple initial poses. +The arrangement of multiple initial poses is efficiently limited by the Hessian matrix of the NDT score function. +In this implementation, the number of initial positions is fixed to simplify the code. +The covariance can be seen as error ellipse from ndt_pose_with_covariance setting on rviz2. +[original paper](https://www.fujipress.jp/jrm/rb/robot003500020435/). + +Note that this function may spoil healthy system behavior if it consumes much calculation resources. + +### Parameters + +initial_pose_offset_model is rotated around (x,y) = (0,0) in the direction of the first principal component of the Hessian matrix. +initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. + +| Name | Type | Description | +| ----------------------------- | ------------------- | ----------------------------------------------------------------- | +| `use_covariance_estimation` | bool | Flag for using real-time covariance estimation (FALSE by default) | +| `initial_pose_offset_model_x` | std::vector | X-axis offset [m] | +| `initial_pose_offset_model_y` | std::vector | Y-axis offset [m] | 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 0ba1b1a2e15f4..1a6c26cd9c351 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -72,6 +72,14 @@ 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, ] + # 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value) + use_covariance_estimation: false + + # Offset arrangement in covariance estimation [m] + # initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. + initial_pose_offset_model_x: [0.0, 0.0, 0.5, -0.5, 1.0, -1.0] + initial_pose_offset_model_y: [0.5, -0.5, 0.0, 0.0, 0.0, 0.0] + # Regularization switch regularization_enabled: false diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index e5f46b5022b51..9eb62230dad9e 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -22,8 +22,10 @@ #include "ndt_scan_matcher/map_update_module.hpp" #include +#include #include +#include #include #include #include @@ -104,7 +106,7 @@ class NDTScanMatcher : public rclcpp::Node const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg); void publish_pose( const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg, - const bool is_converged); + const std::array & ndt_covariance, const bool is_converged); void publish_point_cloud( const rclcpp::Time & sensor_ros_time, const std::string & frame_id, const pcl::shared_ptr> & sensor_points_in_map_ptr); @@ -122,6 +124,10 @@ class NDTScanMatcher : public rclcpp::Node bool validate_converged_param( const double & transform_probability, const double & nearest_voxel_transformation_likelihood); + std::array estimate_covariance( + const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, + const rclcpp::Time & sensor_ros_time); + std::optional interpolate_regularization_pose( const rclcpp::Time & sensor_ros_time); void add_regularization_pose(const rclcpp::Time & sensor_ros_time); @@ -140,6 +146,8 @@ class NDTScanMatcher : public rclcpp::Node ndt_pose_with_covariance_pub_; rclcpp::Publisher::SharedPtr initial_pose_with_covariance_pub_; + rclcpp::Publisher::SharedPtr multi_ndt_pose_pub_; + rclcpp::Publisher::SharedPtr multi_initial_pose_pub_; rclcpp::Publisher::SharedPtr exe_time_pub_; rclcpp::Publisher::SharedPtr transform_probability_pub_; rclcpp::Publisher::SharedPtr @@ -186,6 +194,8 @@ class NDTScanMatcher : public rclcpp::Node double initial_pose_distance_tolerance_m_; float inversion_vector_threshold_; float oscillation_threshold_; + bool use_cov_estimation_; + std::vector initial_pose_offset_model_; std::array output_pose_covariance_; std::deque @@ -204,6 +214,7 @@ class NDTScanMatcher : public rclcpp::Node std::shared_ptr tf2_listener_module_; std::unique_ptr map_module_; std::unique_ptr map_update_module_; + std::unique_ptr logger_configure_; // cspell: ignore degrounded bool estimate_scores_for_degrounded_scan_; diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index a9083c4ae0ae4..dcfdd77cb5a18 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -18,6 +18,7 @@ diagnostic_msgs fmt geometry_msgs + libgoogle-glog-dev libpcl-all-dev localization_util nav_msgs 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 b38e3f0826184..cb66998147220 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -53,6 +53,32 @@ tier4_debug_msgs::msg::Int32Stamped make_int32_stamped( return tier4_debug_msgs::build().stamp(stamp).data(data); } +std::vector create_initial_pose_offset_model( + const std::vector & x, const std::vector & y) +{ + int size = x.size(); + std::vector initial_pose_offset_model(size); + for (int i = 0; i < size; i++) { + initial_pose_offset_model[i].x() = x[i]; + initial_pose_offset_model[i].y() = y[i]; + } + + return initial_pose_offset_model; +} + +Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes( + const Eigen::Matrix2d & matrix) +{ + const Eigen::SelfAdjointEigenSolver eigensolver(matrix); + if (eigensolver.info() == Eigen::Success) { + const Eigen::Vector2d eigen_vec = eigensolver.eigenvectors().col(0); + const double th = std::atan2(eigen_vec.y(), eigen_vec.x()); + return Eigen::Rotation2Dd(th).toRotationMatrix(); + } else { + throw std::runtime_error("Eigen solver failed. Return output_pose_covariance value."); + } +} + bool validate_local_optimal_solution_oscillation( const std::vector & result_pose_msg_array, const float oscillation_threshold, const float inversion_vector_threshold) @@ -141,6 +167,24 @@ NDTScanMatcher::NDTScanMatcher() initial_pose_distance_tolerance_m_ = this->declare_parameter("initial_pose_distance_tolerance_m"); + use_cov_estimation_ = this->declare_parameter("use_covariance_estimation"); + if (use_cov_estimation_) { + std::vector initial_pose_offset_model_x = + this->declare_parameter>("initial_pose_offset_model_x"); + std::vector initial_pose_offset_model_y = + this->declare_parameter>("initial_pose_offset_model_y"); + + if (initial_pose_offset_model_x.size() == initial_pose_offset_model_y.size()) { + initial_pose_offset_model_ = + create_initial_pose_offset_model(initial_pose_offset_model_x, initial_pose_offset_model_y); + } else { + RCLCPP_WARN( + get_logger(), + "Invalid initial pose offset model parameters. Disable covariance estimation."); + use_cov_estimation_ = false; + } + } + std::vector output_pose_covariance = this->declare_parameter>("output_pose_covariance"); for (std::size_t i = 0; i < output_pose_covariance.size(); ++i) { @@ -191,6 +235,9 @@ NDTScanMatcher::NDTScanMatcher() initial_pose_with_covariance_pub_ = this->create_publisher( "initial_pose_with_covariance", 10); + multi_ndt_pose_pub_ = this->create_publisher("multi_ndt_pose", 10); + multi_initial_pose_pub_ = + this->create_publisher("multi_initial_pose", 10); exe_time_pub_ = this->create_publisher("exe_time_ms", 10); transform_probability_pub_ = this->create_publisher("transform_probability", 10); @@ -246,6 +293,8 @@ NDTScanMatcher::NDTScanMatcher() } else { map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group); } + + logger_configure_ = std::make_unique(this); } void NDTScanMatcher::timer_diagnostic() @@ -435,11 +484,6 @@ void NDTScanMatcher::callback_sensor_points( const pclomp::NdtResult ndt_result = ndt_ptr_->getResult(); (*state_ptr_)["state"] = "Sleeping"; - const auto exe_end_time = std::chrono::system_clock::now(); - 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.0f; - const geometry_msgs::msg::Pose result_pose_msg = matrix4f_to_pose(ndt_result.pose); std::vector transformation_msg_array; for (const auto & pose_matrix : ndt_result.transformation_array) { @@ -466,6 +510,19 @@ void NDTScanMatcher::callback_sensor_points( RCLCPP_WARN(get_logger(), "Not Converged"); } + // covariance estimation + std::array ndt_covariance = output_pose_covariance_; + if (is_converged && use_cov_estimation_) { + const auto estimated_covariance = + estimate_covariance(ndt_result, initial_pose_matrix, sensor_ros_time); + ndt_covariance = estimated_covariance; + } + + const auto exe_end_time = std::chrono::system_clock::now(); + 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.0f; + // publish initial_pose_with_covariance_pub_->publish(interpolator.get_current_pose()); exe_time_pub_->publish(make_float32_stamped(sensor_ros_time, exe_time)); @@ -475,7 +532,7 @@ void NDTScanMatcher::callback_sensor_points( make_float32_stamped(sensor_ros_time, ndt_result.nearest_voxel_transformation_likelihood)); iteration_num_pub_->publish(make_int32_stamped(sensor_ros_time, ndt_result.iteration_num)); publish_tf(sensor_ros_time, result_pose_msg); - publish_pose(sensor_ros_time, result_pose_msg, is_converged); + publish_pose(sensor_ros_time, result_pose_msg, ndt_covariance, is_converged); publish_marker(sensor_ros_time, transformation_msg_array); publish_initial_to_result( sensor_ros_time, result_pose_msg, interpolator.get_current_pose(), interpolator.get_old_pose(), @@ -558,7 +615,7 @@ void NDTScanMatcher::publish_tf( void NDTScanMatcher::publish_pose( const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg, - const bool is_converged) + const std::array & ndt_covariance, const bool is_converged) { geometry_msgs::msg::PoseStamped result_pose_stamped_msg; result_pose_stamped_msg.header.stamp = sensor_ros_time; @@ -569,7 +626,7 @@ void NDTScanMatcher::publish_pose( result_pose_with_cov_msg.header.stamp = sensor_ros_time; result_pose_with_cov_msg.header.frame_id = map_frame_; result_pose_with_cov_msg.pose.pose = result_pose_msg; - result_pose_with_cov_msg.pose.covariance = output_pose_covariance_; + result_pose_with_cov_msg.pose.covariance = ndt_covariance; if (is_converged) { ndt_pose_pub_->publish(result_pose_stamped_msg); @@ -692,6 +749,78 @@ bool NDTScanMatcher::validate_converged_param( return is_ok_converged_param; } +std::array NDTScanMatcher::estimate_covariance( + const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, + const rclcpp::Time & sensor_ros_time) +{ + Eigen::Matrix2d rot = Eigen::Matrix2d::Identity(); + try { + rot = find_rotation_matrix_aligning_covariance_to_principal_axes( + ndt_result.hessian.inverse().block(0, 0, 2, 2)); + } catch (const std::exception & e) { + RCLCPP_WARN(get_logger(), e.what()); + return output_pose_covariance_; + } + + // first result is added to mean + const int n = initial_pose_offset_model_.size() + 1; + const Eigen::Vector2d ndt_pose_2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3)); + Eigen::Vector2d mean = ndt_pose_2d; + std::vector ndt_pose_2d_vec; + ndt_pose_2d_vec.reserve(n); + ndt_pose_2d_vec.emplace_back(ndt_pose_2d); + + geometry_msgs::msg::PoseArray multi_ndt_result_msg; + geometry_msgs::msg::PoseArray multi_initial_pose_msg; + multi_ndt_result_msg.header.stamp = sensor_ros_time; + multi_ndt_result_msg.header.frame_id = map_frame_; + multi_initial_pose_msg.header.stamp = sensor_ros_time; + multi_initial_pose_msg.header.frame_id = map_frame_; + multi_ndt_result_msg.poses.push_back(matrix4f_to_pose(ndt_result.pose)); + multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(initial_pose_matrix)); + + // multiple searches + for (const auto & pose_offset : initial_pose_offset_model_) { + const Eigen::Vector2d rotated_pose_offset_2d = rot * pose_offset; + + Eigen::Matrix4f sub_initial_pose_matrix(Eigen::Matrix4f::Identity()); + sub_initial_pose_matrix = ndt_result.pose; + sub_initial_pose_matrix(0, 3) += static_cast(rotated_pose_offset_2d.x()); + sub_initial_pose_matrix(1, 3) += static_cast(rotated_pose_offset_2d.y()); + + auto sub_output_cloud = std::make_shared>(); + ndt_ptr_->align(*sub_output_cloud, sub_initial_pose_matrix); + const Eigen::Matrix4f sub_ndt_result = ndt_ptr_->getResult().pose; + + const Eigen::Vector2d sub_ndt_pose_2d = sub_ndt_result.topRightCorner<2, 1>().cast(); + mean += sub_ndt_pose_2d; + ndt_pose_2d_vec.emplace_back(sub_ndt_pose_2d); + + multi_ndt_result_msg.poses.push_back(matrix4f_to_pose(sub_ndt_result)); + multi_initial_pose_msg.poses.push_back(matrix4f_to_pose(sub_initial_pose_matrix)); + } + + // calculate the covariance matrix + mean /= n; + Eigen::Matrix2d pca_covariance = Eigen::Matrix2d::Zero(); + for (const auto & temp_ndt_pose_2d : ndt_pose_2d_vec) { + const Eigen::Vector2d diff_2d = temp_ndt_pose_2d - mean; + pca_covariance += diff_2d * diff_2d.transpose(); + } + pca_covariance /= (n - 1); // unbiased covariance + + std::array ndt_covariance = output_pose_covariance_; + ndt_covariance[0 + 6 * 0] += pca_covariance(0, 0); + ndt_covariance[1 + 6 * 0] += pca_covariance(1, 0); + ndt_covariance[0 + 6 * 1] += pca_covariance(0, 1); + ndt_covariance[1 + 6 * 1] += pca_covariance(1, 1); + + multi_ndt_pose_pub_->publish(multi_ndt_result_msg); + multi_initial_pose_pub_->publish(multi_initial_pose_msg); + + return ndt_covariance; +} + std::optional NDTScanMatcher::interpolate_regularization_pose( const rclcpp::Time & sensor_ros_time) { diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp index be6b398d7738e..d5ea544d3c5e5 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp @@ -16,8 +16,13 @@ #include +#include + int main(int argc, char ** argv) { + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + rclcpp::init(argc, argv); auto ndt_scan_matcher = std::make_shared(); rclcpp::executors::MultiThreadedExecutor exec; diff --git a/localization/pose_initializer/package.xml b/localization/pose_initializer/package.xml index f8333d89fa8ac..1a1a8e48b7e3b 100644 --- a/localization/pose_initializer/package.xml +++ b/localization/pose_initializer/package.xml @@ -21,6 +21,7 @@ motion_utils rclcpp std_srvs + tier4_autoware_utils tier4_localization_msgs ament_cmake_cppcheck diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp index 6614cbf56008e..4585414c20b0d 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -54,6 +54,7 @@ PoseInitializer::PoseInitializer() : Node("pose_initializer") stop_check_duration_ = declare_parameter("stop_check_duration"); stop_check_ = std::make_unique(this, stop_check_duration_ + 1.0); } + logger_configure_ = std::make_unique(this); change_state(State::Message::UNINITIALIZED); } diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp index fa04146cf529e..014c8e9bf6e04 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include @@ -55,6 +56,7 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr stop_check_; std::unique_ptr ekf_localization_trigger_; std::unique_ptr ndt_localization_trigger_; + std::unique_ptr logger_configure_; double stop_check_duration_; void change_state(State::Message::_state_type state); void on_initialize( diff --git a/map/map_loader/README.md b/map/map_loader/README.md index e39b6d66af86c..65d9594cb7415 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -14,6 +14,8 @@ Currently, it supports the following two types: - Send partial pointcloud map loading via ROS 2 service - Send differential pointcloud map loading via ROS 2 service +NOTE: **We strongly recommend to use divided maps when using large pointcloud map to enable the latter two features (partial and differential load). Please go through the prerequisites section for more details, and follow the instruction for dividing the map and preparing the metadata.** + ### Prerequisites #### Prerequisites on pointcloud map file(s) diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index 53794fb93b7c6..082dc95d6500a 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -125,15 +125,18 @@ void Lanelet2MapVisualizationNode::onMapBin( viz_lanelet_map, "no_obstacle_segmentation_area_for_run_out"); lanelet::ConstPolygons3d hatched_road_markings_area = lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "hatched_road_markings"); + lanelet::ConstPolygons3d intersection_areas = + lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "intersection_area"); std::vector no_parking_reg_elems = lanelet::utils::query::noParkingAreas(all_lanelets); + lanelet::ConstLineStrings3d curbstones = lanelet::utils::query::curbstones(viz_lanelet_map); std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings, cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas, cl_speed_bumps, cl_crosswalks, cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons, cl_no_stopping_areas, cl_no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area_for_run_out, cl_hatched_road_markings_area, - cl_hatched_road_markings_line, cl_no_parking_areas; + cl_hatched_road_markings_line, cl_no_parking_areas, cl_curbstones, cl_intersection_area; setColor(&cl_road, 0.27, 0.27, 0.27, 0.999); setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999); setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5); @@ -156,6 +159,8 @@ void Lanelet2MapVisualizationNode::onMapBin( setColor(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5); setColor(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999); setColor(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5); + setColor(&cl_curbstones, 0.1, 0.1, 0.2, 0.999); + setColor(&cl_intersection_area, 0.16, 1.0, 0.69, 0.5); visualization_msgs::msg::MarkerArray map_marker_array; @@ -242,6 +247,14 @@ void Lanelet2MapVisualizationNode::onMapBin( &map_marker_array, lanelet::visualization::noParkingAreasAsMarkerArray(no_parking_reg_elems, cl_no_parking_areas)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::lineStringsAsMarkerArray(curbstones, "curbstone", cl_curbstones, 0.2)); + + insertMarkerArray( + &map_marker_array, lanelet::visualization::intersectionAreaAsMarkerArray( + intersection_areas, cl_intersection_area)); + pub_marker_->publish(map_marker_array); } diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index abdd44a70d24a..60f6f943b8cda 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -29,7 +29,6 @@ - diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml index d7ff85adc9193..6a1354b37928f 100644 --- a/perception/map_based_prediction/package.xml +++ b/perception/map_based_prediction/package.xml @@ -4,7 +4,6 @@ map_based_prediction 0.1.0 This package implements a map_based_prediction - Yutaka Shimizu Tomoya Kimura Shunsuke Miura Yoshi Ri 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 08fc06330b1d8..2ceb22fd6e7b9 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -687,6 +687,32 @@ lanelet::Lanelets getLeftOppositeLanelets( return opposite_lanelets; } + +void replaceObjectYawWithLaneletsYaw( + const LaneletsData & current_lanelets, TrackedObject & transformed_object) +{ + // return if no lanelet is found + if (current_lanelets.empty()) return; + auto & pose_with_cov = transformed_object.kinematics.pose_with_covariance; + // for each lanelet, calc lanelet angle and calculate mean angle + double sum_x = 0.0; + double sum_y = 0.0; + for (const auto & current_lanelet : current_lanelets) { + const auto lanelet_angle = + lanelet::utils::getLaneletAngle(current_lanelet.lanelet, pose_with_cov.pose.position); + sum_x += std::cos(lanelet_angle); + sum_y += std::sin(lanelet_angle); + } + const double mean_yaw_angle = std::atan2(sum_y, sum_x); + double roll, pitch, yaw; + tf2::Quaternion original_quaternion; + tf2::fromMsg(pose_with_cov.pose.orientation, original_quaternion); + tf2::Matrix3x3(original_quaternion).getRPY(roll, pitch, yaw); + tf2::Quaternion filtered_quaternion; + filtered_quaternion.setRPY(roll, pitch, mean_yaw_angle); + pose_with_cov.pose.orientation = tf2::toMsg(filtered_quaternion); +} + } // namespace MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_options) @@ -929,11 +955,19 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt getDebugMarker(object, max_prob_path->maneuver, debug_markers.markers.size()); debug_markers.markers.push_back(debug_marker); + // Fix object angle if its orientation unreliable (e.g. far object by radar sensor) + // This prevent bending predicted path + TrackedObject yaw_fixed_transformed_object = transformed_object; + if ( + transformed_object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE) { + replaceObjectYawWithLaneletsYaw(current_lanelets, yaw_fixed_transformed_object); + } // Generate Predicted Path std::vector predicted_paths; for (const auto & ref_path : ref_paths) { - PredictedPath predicted_path = - path_generator_->generatePathForOnLaneVehicle(transformed_object, ref_path.path); + PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle( + yaw_fixed_transformed_object, ref_path.path); if (predicted_path.path.empty()) { continue; } diff --git a/perception/radar_object_tracker/CMakeLists.txt b/perception/radar_object_tracker/CMakeLists.txt index 424e2b2d2c13e..3e3afacadd00f 100644 --- a/perception/radar_object_tracker/CMakeLists.txt +++ b/perception/radar_object_tracker/CMakeLists.txt @@ -27,6 +27,7 @@ ament_auto_add_library(radar_object_tracker_node SHARED src/radar_object_tracker_node/radar_object_tracker_node.cpp src/tracker/model/tracker_base.cpp src/tracker/model/linear_motion_tracker.cpp + src/tracker/model/constant_turn_rate_motion_tracker.cpp src/utils/utils.cpp src/data_association/data_association.cpp ) diff --git a/perception/radar_object_tracker/README.md b/perception/radar_object_tracker/README.md index 739a4a745ff6c..395bc025c2821 100644 --- a/perception/radar_object_tracker/README.md +++ b/perception/radar_object_tracker/README.md @@ -46,7 +46,7 @@ See more details in the [models.md](models.md). | `publish_rate` | double | 10.0 | The rate at which to publish the output messages | | `world_frame_id` | string | "map" | The frame ID of the world coordinate system | | `enable_delay_compensation` | bool | false | Whether to enable delay compensation. If set to `true`, output topic is published by timer with `publish_rate`. | -| `tracking_config_directory` | string | "" | The directory containing the tracking configuration files | +| `tracking_config_directory` | string | "./config/tracking/" | The directory containing the tracking configuration files | | `enable_logging` | bool | false | Whether to enable logging | | `logging_file_path` | string | "/tmp/association_log.json" | The path to the file where logs should be written | | `tracker_lifetime` | double | 1.0 | The lifetime of the tracker in seconds | @@ -65,6 +65,15 @@ See more details in the [models.md](models.md). See more details in the [models.md](models.md). +### Tracker parameters + +Currently, this package supports the following trackers: + +- `linear_motion_tracker` +- `constant_turn_rate_motion_tracker` + +Default settings for each tracker are defined in the [./config/tracking/](./config/tracking/), and described in [models.md](models.md). + ## Assumptions / Known limits diff --git a/perception/radar_object_tracker/config/data_association_matrix.param.yaml b/perception/radar_object_tracker/config/data_association_matrix.param.yaml index 104d790d185db..69628414e67d4 100644 --- a/perception/radar_object_tracker/config/data_association_matrix.param.yaml +++ b/perception/radar_object_tracker/config/data_association_matrix.param.yaml @@ -14,10 +14,10 @@ max_dist_matrix: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN [4.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, #UNKNOWN - 4.0, 4.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #CAR - 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRUCK - 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #BUS - 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRAILER + 4.0, 8.0, 8.0, 8.0, 8.0, 1.0, 1.0, 1.0, #CAR + 4.0, 6.0, 8.0, 8.0, 8.0, 1.0, 1.0, 1.0, #TRUCK + 4.0, 6.0, 8.0, 8.0, 8.0, 1.0, 1.0, 1.0, #BUS + 4.0, 6.0, 8.0, 8.0, 8.0, 1.0, 1.0, 1.0, #TRAILER 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #MOTORBIKE 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #BICYCLE 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0] #PEDESTRIAN diff --git a/perception/radar_object_tracker/config/default_tracker.param.yaml b/perception/radar_object_tracker/config/default_tracker.param.yaml index 757125202d69b..6c26034860e1b 100644 --- a/perception/radar_object_tracker/config/default_tracker.param.yaml +++ b/perception/radar_object_tracker/config/default_tracker.param.yaml @@ -1,9 +1,9 @@ /**: ros__parameters: - car_tracker: "linear_motion_tracker" - truck_tracker: "linear_motion_tracker" - bus_tracker: "linear_motion_tracker" - trailer_tracker: "linear_motion_tracker" - pedestrian_tracker: "linear_motion_tracker" - bicycle_tracker: "linear_motion_tracker" - motorcycle_tracker: "linear_motion_tracker" + car_tracker: "constant_turn_rate_motion_tracker" + truck_tracker: "constant_turn_rate_motion_tracker" + bus_tracker: "constant_turn_rate_motion_tracker" + trailer_tracker: "constant_turn_rate_motion_tracker" + pedestrian_tracker: "constant_turn_rate_motion_tracker" + bicycle_tracker: "constant_turn_rate_motion_tracker" + motorcycle_tracker: "constant_turn_rate_motion_tracker" diff --git a/perception/radar_object_tracker/config/radar_object_tracker.param.yaml b/perception/radar_object_tracker/config/radar_object_tracker.param.yaml index f80adffb41090..d2c0841cf372e 100644 --- a/perception/radar_object_tracker/config/radar_object_tracker.param.yaml +++ b/perception/radar_object_tracker/config/radar_object_tracker.param.yaml @@ -4,12 +4,11 @@ # basic settings world_frame_id: "map" tracker_lifetime: 1.0 # [sec] - # if empty, use default config declared in this package - tracking_config_directory: "" + measurement_count_threshold: 3 # object will be published if it is tracked more than this threshold # delay compensate parameters publish_rate: 10.0 - enable_delay_compensation: false + enable_delay_compensation: true # logging enable_logging: false @@ -18,10 +17,10 @@ # filtering ## 1. distance based filtering: remove closer objects than this threshold use_distance_based_noise_filtering: true - minimum_range_threshold: 70.0 # [m] + minimum_range_threshold: 60.0 # [m] ## 2. lanelet map based filtering use_map_based_noise_filtering: true max_distance_from_lane: 5.0 # [m] max_angle_diff_from_lane: 0.785398 # [rad] (45 deg) - max_lateral_velocity: 5.0 # [m/s] + max_lateral_velocity: 7.0 # [m/s] diff --git a/perception/radar_object_tracker/config/tracking/constant_turn_rate_motion_tracker.yaml b/perception/radar_object_tracker/config/tracking/constant_turn_rate_motion_tracker.yaml new file mode 100644 index 0000000000000..f80f881cabf03 --- /dev/null +++ b/perception/radar_object_tracker/config/tracking/constant_turn_rate_motion_tracker.yaml @@ -0,0 +1,36 @@ +default: + # This file defines the parameters for the linear motion tracker. + # All this parameter coordinate is assumed to be in the vehicle coordinate system. + # So, the x axis is pointing to the front of the vehicle, y axis is pointing to the left of the vehicle. + ekf_params: + # random walk noise is used to model the acceleration noise + process_noise_std: # [m/s^2] + x: 0.5 + y: 0.5 + yaw: 0.1 + vx: 1.0 # assume 1m/s velocity noise + wz: 0.4 + measurement_noise_std: + x: 4.0 # [m] + y: 4.0 # [m] + # y: 0.02 # rad/m if use_polar_coordinate_in_measurement_noise is true + yaw: 0.2 # [rad] + vx: 10 # [m/s] + initial_covariance_std: + x: 3.0 # [m] + y: 6.0 # [m] + yaw: 10.0 # [rad] + vx: 100.0 # [m/s] + wz: 10.0 # [rad/s] + # input flag + trust_yaw_input: false # set true if yaw input of sensor is reliable + trust_twist_input: false # set true if twist input of sensor is reliable + use_polar_coordinate_in_measurement_noise: false # set true if you want to define the measurement noise in polar coordinate + assume_zero_yaw_rate: false # set true if you want to assume zero yaw rate + # output limitation + limit: + max_speed: 80.0 # [m/s] + # low pass filter is used to smooth the yaw and shape estimation + low_pass_filter: + time_constant: 1.0 # [s] + sampling_time: 0.1 # [s] diff --git a/perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml b/perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml index 71367f4575193..5e813558a2bff 100644 --- a/perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml +++ b/perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml @@ -7,22 +7,28 @@ default: process_noise_std: # [m/s^2] ax: 0.98 # assume 0.1G acceleration noise ay: 0.98 - vx: 0.1 # assume 0.1m/s velocity noise - vy: 0.1 + vx: 1.0 # assume 1m/s velocity noise + vy: 1.0 x: 1.0 # assume 1m position noise y: 1.0 measurement_noise_std: x: 0.6 # [m] - y: 0.9 # [m] - vx: 0.4 # [m/s] - vy: 1 # [m/s] + # y: 4.0 # [m] + y: 0.01 # rad/m if use_polar_coordinate_in_measurement_noise is true + vx: 10 # [m/s] + vy: 100 # [m/s] initial_covariance_std: x: 3.0 # [m] y: 6.0 # [m] - vx: 1.0 # [m/s] - vy: 5.0 # [m/s] - ax: 0.5 # [m/s^2] - ay: 1.0 # [m/s^2] + vx: 1000.0 # [m/s] + vy: 1000.0 # [m/s] + ax: 1000.0 # [m/s^2] + ay: 1000.0 # [m/s^2] + estimate_acc: false # set true if you want to estimate the acceleration + # input flag + trust_yaw_input: false # set true if yaw input of sensor is reliable + trust_twist_input: false # set true if twist input of sensor is reliable + use_polar_coordinate_in_measurement_noise: true # set true if you want to define the measurement noise in polar coordinate # output limitation limit: max_speed: 80.0 # [m/s] diff --git a/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp b/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp index 415ff24b34cc3..9a3fdf602a3ca 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp +++ b/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp @@ -76,6 +76,8 @@ class RadarObjectTrackerNode : public rclcpp::Node float tracker_lifetime_; std::map tracker_map_; + int measurement_count_threshold_; + void onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); void onTimer(); diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp b/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp new file mode 100644 index 0000000000000..ac4b2cd1a0acb --- /dev/null +++ b/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp @@ -0,0 +1,112 @@ +// Copyright 2020 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 RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_ +#define RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_ + +#include "radar_object_tracker/tracker/model/tracker_base.hpp" + +#include + +#include + +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; +class ConstantTurnRateMotionTracker : public Tracker // means constant turn rate motion tracker +{ +private: + autoware_auto_perception_msgs::msg::DetectedObject object_; + 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 + { + // dimension + char dim_x = 5; + // system noise + double q_cov_x; + double q_cov_y; + double q_cov_yaw; + double q_cov_vx; + double q_cov_wz; + // measurement noise + double r_cov_x; + double r_cov_y; + double r_cov_yaw; + double r_cov_vx; + // initial state covariance + double p0_cov_x; + double p0_cov_y; + double p0_cov_yaw; + double p0_cov_vx; + double p0_cov_wz; + }; + static EkfParams ekf_params_; + + // limitation + static double max_vx_; + // rough tracking parameters + float z_; + + // lpf parameter + static double filter_tau_; // time constant of 1st order low pass filter + static double filter_dt_; // sampling time of 1st order low pass filter + + // static flags + static bool is_initialized_; + static bool trust_yaw_input_; + static bool trust_twist_input_; + static bool use_polar_coordinate_in_measurement_noise_; + static bool assume_zero_yaw_rate_; + +private: + struct BoundingBox + { + double length; + double width; + double height; + }; + struct Cylinder + { + double width; + double height; + }; + BoundingBox bounding_box_; + Cylinder cylinder_; + +public: + ConstantTurnRateMotionTracker( + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const std::string & tracker_param_file, const std::uint8_t & label); + + static void loadDefaultModelParameters(const std::string & path); + 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; + bool measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); + 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; + virtual ~ConstantTurnRateMotionTracker() {} +}; + +#endif // RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp b/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp index 108e79c043ba0..77caa7a266481 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp +++ b/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp @@ -56,18 +56,25 @@ class LinearMotionTracker : public Tracker double p0_cov_vy; double p0_cov_ax; double p0_cov_ay; - } ekf_params_; + }; + static EkfParams ekf_params_; // limitation - double max_vx_; - double max_vy_; + static double max_vx_; + static double max_vy_; // rough tracking parameters float z_; float yaw_; // lpf parameter - double filter_tau_; // time constant of 1st order low pass filter - double filter_dt_; // sampling time of 1st order low pass filter + static double filter_tau_; // time constant of 1st order low pass filter + static double filter_dt_; // sampling time of 1st order low pass filter + + static bool is_initialized_; + static bool estimate_acc_; + static bool trust_yaw_input_; + static bool trust_twist_input_; + static bool use_polar_coordinate_in_measurement_noise_; private: struct BoundingBox @@ -89,13 +96,15 @@ class LinearMotionTracker : public Tracker const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, const std::string & tracker_param_file, const std::uint8_t & label); - void loadDefaultModelParameters(const std::string & path); + static void loadDefaultModelParameters(const std::string & path); 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; - bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); + bool measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp b/perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp index 7da940912a253..70045e6b16a07 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp +++ b/perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp @@ -15,6 +15,7 @@ #ifndef RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ #define RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ +#include "model/constant_turn_rate_motion_tracker.hpp" #include "model/linear_motion_tracker.hpp" #include "model/tracker_base.hpp" diff --git a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml index e2d30c1b69d19..6e6813d3e40ff 100644 --- a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml +++ b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml @@ -4,8 +4,9 @@ - + + @@ -17,6 +18,7 @@ + diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp index 6b02836e1f933..6d801302ab6c5 100644 --- a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp +++ b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp @@ -207,6 +207,7 @@ RadarObjectTrackerNode::RadarObjectTrackerNode(const rclcpp::NodeOptions & node_ // Parameters tracker_lifetime_ = declare_parameter("tracker_lifetime"); double publish_rate = declare_parameter("publish_rate"); + measurement_count_threshold_ = declare_parameter("measurement_count_threshold"); world_frame_id_ = declare_parameter("world_frame_id"); bool enable_delay_compensation{declare_parameter("enable_delay_compensation")}; tracker_config_directory_ = declare_parameter("tracking_config_directory"); @@ -363,6 +364,10 @@ std::shared_ptr RadarObjectTrackerNode::createNewTracker( if (tracker == "linear_motion_tracker") { std::string config_file = tracker_config_directory_ + "linear_motion_tracker.yaml"; return std::make_shared(time, object, config_file, label); + } else if (tracker == "constant_turn_rate_motion_tracker") { + std::string config_file = + tracker_config_directory_ + "constant_turn_rate_motion_tracker.yaml"; + return std::make_shared(time, object, config_file, label); } else { // not implemented yet so put warning RCLCPP_WARN(get_logger(), "Tracker %s is not implemented yet", tracker.c_str()); @@ -530,8 +535,7 @@ void RadarObjectTrackerNode::sanitizeTracker( inline bool RadarObjectTrackerNode::shouldTrackerPublish( const std::shared_ptr tracker) const { - constexpr int measurement_count_threshold = 3; - if (tracker->getTotalMeasurementCount() < measurement_count_threshold) { + if (tracker->getTotalMeasurementCount() < measurement_count_threshold_) { return false; } return true; diff --git a/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp new file mode 100644 index 0000000000000..5815fe34a68a9 --- /dev/null +++ b/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp @@ -0,0 +1,624 @@ +// Copyright 2020 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 Yukihiro Saito +// + +#include "radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp" + +#include "radar_object_tracker/utils/utils.hpp" + +#include +#include + +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#define EIGEN_MPL2_ONLY +#include +#include +#include +#include + +#include + +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + +// init static member variables +bool ConstantTurnRateMotionTracker::is_initialized_ = false; +ConstantTurnRateMotionTracker::EkfParams ConstantTurnRateMotionTracker::ekf_params_; +double ConstantTurnRateMotionTracker::max_vx_; +double ConstantTurnRateMotionTracker::filter_tau_; +double ConstantTurnRateMotionTracker::filter_dt_; +bool ConstantTurnRateMotionTracker::assume_zero_yaw_rate_; +bool ConstantTurnRateMotionTracker::trust_yaw_input_; +bool ConstantTurnRateMotionTracker::trust_twist_input_; +bool ConstantTurnRateMotionTracker::use_polar_coordinate_in_measurement_noise_; + +ConstantTurnRateMotionTracker::ConstantTurnRateMotionTracker( + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const std::string & tracker_param_file, const std::uint8_t & /*label*/) +: Tracker(time, object.classification), + logger_(rclcpp::get_logger("ConstantTurnRateMotionTracker")), + last_update_time_(time), + z_(object.kinematics.pose_with_covariance.pose.position.z) +{ + object_ = object; + + // load setting from yaml file + if (!is_initialized_) { + loadDefaultModelParameters(tracker_param_file); // currently not using label + is_initialized_ = true; + } + + // shape initialization + bounding_box_ = {0.5, 0.5, 1.7}; + cylinder_ = {0.3, 1.7}; + + // initialize X matrix and position + 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; + const auto yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + z_ = object.kinematics.pose_with_covariance.pose.position.z; + X(IDX::YAW) = yaw; + // radar object usually have twist + if (object.kinematics.has_twist) { + const auto v = object.kinematics.twist_with_covariance.twist.linear.x; + X(IDX::VX) = v; + } else { + X(IDX::VX) = 0.0; + } + // init turn rate + X(IDX::WZ) = 0.0; + + // initialize P matrix + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + + // create rotation matrix to rotate covariance matrix + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + // 2d rotation matrix + Eigen::Matrix2d R; + R << cos_yaw, -sin_yaw, sin_yaw, cos_yaw; + + // covariance matrix in the target vehicle coordinate system + Eigen::Matrix2d P_xy_local; + P_xy_local << ekf_params_.p0_cov_x, 0.0, 0.0, ekf_params_.p0_cov_y; + + // Rotated covariance matrix + // covariance is rotated by 2D rotation matrix R + // P′=R P RT + Eigen::Matrix2d P_xy, P_v_xy; + + // Rotate the covariance matrix according to the vehicle yaw + // because p0_cov_x and y are in the vehicle coordinate system. + if (object.kinematics.has_position_covariance) { + P_xy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X], + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y], + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X], + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + P_xy = R * P_xy_local * R.transpose(); + } else { + // rotate + P_xy = R * P_xy_local * R.transpose(); + } + // put value in P matrix + P.block<2, 2>(IDX::X, IDX::X) = P_xy; + + // covariance often written in object frame + if (object.kinematics.has_twist_covariance) { + const auto vx_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + // const auto vy_cov = + // object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + P(IDX::VX, IDX::VX) = vx_cov; + } else { + P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + } + + P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; + P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; + + // init shape + 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 if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + cylinder_ = {object.shape.dimensions.x, object.shape.dimensions.z}; + } + + ekf_.init(X, P); +} + +void ConstantTurnRateMotionTracker::loadDefaultModelParameters(const std::string & path) +{ + YAML::Node config = YAML::LoadFile(path); + // initialize ekf params + const float q_stddev_x = + config["default"]["ekf_params"]["process_noise_std"]["x"].as(); // [m] + const float q_stddev_y = + config["default"]["ekf_params"]["process_noise_std"]["y"].as(); // [m] + const float q_stddev_yaw = + config["default"]["ekf_params"]["process_noise_std"]["yaw"].as(); // [rad] + const float q_stddev_vx = + config["default"]["ekf_params"]["process_noise_std"]["vx"].as(); // [m/s] + const float q_stddev_wz = + config["default"]["ekf_params"]["process_noise_std"]["wz"].as(); // [m/s] + const float r_stddev_x = + config["default"]["ekf_params"]["measurement_noise_std"]["x"].as(); // [m] + const float r_stddev_y = + config["default"]["ekf_params"]["measurement_noise_std"]["y"].as(); // [m] + const float r_stddev_yaw = + config["default"]["ekf_params"]["measurement_noise_std"]["yaw"].as(); // [rad] + const float r_stddev_vx = + config["default"]["ekf_params"]["measurement_noise_std"]["vx"].as(); // [m/s] + const float p0_stddev_x = + config["default"]["ekf_params"]["initial_covariance_std"]["x"].as(); // [m/s] + const float p0_stddev_y = + config["default"]["ekf_params"]["initial_covariance_std"]["y"].as(); // [m/s] + const float p0_stddev_yaw = + config["default"]["ekf_params"]["initial_covariance_std"]["yaw"].as(); // [rad] + const float p0_stddev_vx = + config["default"]["ekf_params"]["initial_covariance_std"]["vx"].as(); // [m/(s)] + const float p0_stddev_wz = + config["default"]["ekf_params"]["initial_covariance_std"]["wz"].as(); // [rad/s] + assume_zero_yaw_rate_ = config["default"]["assume_zero_yaw_rate"].as(); // default false + trust_yaw_input_ = config["default"]["trust_yaw_input"].as(); // default false + trust_twist_input_ = config["default"]["trust_twist_input"].as(); // default false + use_polar_coordinate_in_measurement_noise_ = + config["default"]["use_polar_coordinate_in_measurement_noise"].as(); // default false + 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); + 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_vx = std::pow(r_stddev_vx, 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_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); + + // lpf filter parameters + filter_tau_ = config["default"]["low_pass_filter"]["time_constant"].as(); // [s] + filter_dt_ = config["default"]["low_pass_filter"]["sampling_time"].as(); // [s] + + // limitation + // (TODO): this may be written in another yaml file based on classify result + const float max_speed_kmph = config["default"]["limit"]["max_speed"].as(); // [km/h] + max_vx_ = tier4_autoware_utils::kmph2mps(max_speed_kmph); // [m/s] +} + +bool ConstantTurnRateMotionTracker::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; +} + +bool ConstantTurnRateMotionTracker::predict(const double dt, KalmanFilter & ekf) const +{ + /* == Nonlinear model == + * + * 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 + * + */ + + /* == Linearized model == + * + * 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] + */ + + const double yaw_rate_coeff = assume_zero_yaw_rate_ ? 0.0 : 1.0; + + // X t + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state + ekf.getX(X_t); + const auto x = X_t(IDX::X); + const auto y = X_t(IDX::Y); + const auto yaw = X_t(IDX::YAW); + const auto vx = X_t(IDX::VX); + const auto wz = X_t(IDX::WZ); + + // X t+1 + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); + X_next_t(IDX::X) = x + vx * std::cos(yaw) * dt; + X_next_t(IDX::Y) = y + vx * std::sin(yaw) * dt; + X_next_t(IDX::YAW) = yaw + wz * dt * yaw_rate_coeff; + X_next_t(IDX::VX) = vx; + X_next_t(IDX::WZ) = wz * yaw_rate_coeff; + + // A: state transition matrix + Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); + A(IDX::X, IDX::YAW) = -vx * std::sin(yaw) * dt; + A(IDX::Y, IDX::YAW) = vx * std::cos(yaw) * dt; + A(IDX::X, IDX::VX) = std::cos(yaw) * dt; + A(IDX::Y, IDX::VX) = std::sin(yaw) * dt; + A(IDX::YAW, IDX::WZ) = dt * yaw_rate_coeff; + + // Q: system noise + 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. + Eigen::MatrixXd Q_xy_local = Eigen::MatrixXd::Zero(2, 2); + Eigen::MatrixXd Q_xy_global = Eigen::MatrixXd::Zero(2, 2); + Q_xy_local << ekf_params_.q_cov_x, 0.0, 0.0, ekf_params_.q_cov_y; + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(2, 2); + R << cos(yaw), -sin(yaw), sin(yaw), cos(yaw); + Q_xy_global = R * Q_xy_local * R.transpose(); + Q.block<2, 2>(IDX::X, IDX::X) = Q_xy_global; + + Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw; + Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx; + Q(IDX::WZ, IDX::WZ) = ekf_params_.q_cov_wz; + + // may be unused + 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); + + // call kalman filter library + if (!ekf.predict(X_next_t, A, Q)) { + RCLCPP_WARN(logger_, "Cannot predict"); + } + + return true; +} + +bool ConstantTurnRateMotionTracker::measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) +{ + // Observation pattern will be: + // 1. x, y, vx, vy + // 2. x, y + // 3. vx, vy (Do not consider this right now) + // + // We handle this measurements by stacking observation matrix, measurement vector and measurement + // covariance + // - observation matrix: C + // - measurement vector : Y + // - measurement covariance: R + + // get current state + Eigen::MatrixXd X(ekf_params_.dim_x, 1); + ekf_.getX(X); + const auto yaw_state = X(IDX::YAW); + + // rotation matrix + Eigen::Matrix2d RotationYaw; + if (trust_yaw_input_) { + const auto yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + RotationYaw << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); + } else { + RotationYaw << std::cos(yaw_state), -std::sin(yaw_state), std::sin(yaw_state), + std::cos(yaw_state); + } + const auto base_link_yaw = tf2::getYaw(self_transform.rotation); + Eigen::Matrix2d RotationBaseLink; + RotationBaseLink << std::cos(base_link_yaw), -std::sin(base_link_yaw), std::sin(base_link_yaw), + std::cos(base_link_yaw); + + // depth from base_link to object + const auto dx = + object.kinematics.pose_with_covariance.pose.position.x - self_transform.translation.x; + const auto dy = + object.kinematics.pose_with_covariance.pose.position.y - self_transform.translation.y; + Eigen::Vector2d pose_diff_in_map(dx, dy); + Eigen::Vector2d pose_diff_in_base_link = RotationBaseLink * pose_diff_in_map; + const auto depth = abs(pose_diff_in_base_link(0)); + + // gather matrices as vector + std::vector C_list; + std::vector Y_list; + std::vector R_block_list; + + // 1. add position measurement + const bool enable_position_measurement = true; // assume position is always measured + if (enable_position_measurement) { + Eigen::MatrixXd Cxy = Eigen::MatrixXd::Zero(2, ekf_params_.dim_x); + Cxy(0, IDX::X) = 1; + Cxy(1, IDX::Y) = 1; + C_list.push_back(Cxy); + + Eigen::MatrixXd Yxy = Eigen::MatrixXd::Zero(2, 1); + Yxy << object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y; + Y_list.push_back(Yxy); + + // covariance need to be rotated since it is in the vehicle coordinate system + Eigen::MatrixXd Rxy_local = Eigen::MatrixXd::Zero(2, 2); + Eigen::MatrixXd Rxy = Eigen::MatrixXd::Zero(2, 2); + if (!object.kinematics.has_position_covariance) { + // switch noise covariance in polar coordinate or cartesian coordinate + const auto r_cov_y = use_polar_coordinate_in_measurement_noise_ + ? depth * depth * ekf_params_.r_cov_x + : ekf_params_.r_cov_y; + Rxy_local << ekf_params_.r_cov_x, 0, 0, r_cov_y; // xy in base_link coordinate + Rxy = RotationBaseLink * Rxy_local * RotationBaseLink.transpose(); + } else { + Rxy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X], + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y], + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X], + object.kinematics.pose_with_covariance + .covariance[utils::MSG_COV_IDX::Y_Y]; // xy in vehicle coordinate + Rxy = RotationYaw * Rxy_local * RotationYaw.transpose(); + } + R_block_list.push_back(Rxy); + } + + // 2. add yaw measurement + const bool object_has_orientation = object.kinematics.orientation_availability > + 0; // 0: not available, 1: sign unknown, 2: available + const bool enable_yaw_measurement = trust_yaw_input_ && object_has_orientation; + + if (enable_yaw_measurement) { + Eigen::MatrixXd Cyaw = Eigen::MatrixXd::Zero(1, ekf_params_.dim_x); + Cyaw(0, IDX::YAW) = 1; + C_list.push_back(Cyaw); + + Eigen::MatrixXd Yyaw = Eigen::MatrixXd::Zero(1, 1); + const auto yaw = [&] { + auto obj_yaw = tier4_autoware_utils::normalizeRadian( + tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); + while (M_PI_2 <= yaw_state - obj_yaw) { + obj_yaw = obj_yaw + M_PI; + } + while (M_PI_2 <= obj_yaw - yaw_state) { + obj_yaw = obj_yaw - M_PI; + } + return obj_yaw; + }(); + + Yyaw << yaw; + Y_list.push_back(Yyaw); + + Eigen::MatrixXd Ryaw = Eigen::MatrixXd::Zero(1, 1); + Ryaw << ekf_params_.r_cov_yaw; + R_block_list.push_back(Ryaw); + } + + // 3. add linear velocity measurement + const bool enable_velocity_measurement = object.kinematics.has_twist && trust_twist_input_; + if (enable_velocity_measurement) { + Eigen::MatrixXd C_vx = Eigen::MatrixXd::Zero(1, ekf_params_.dim_x); + C_vx(0, IDX::VX) = 1; + C_list.push_back(C_vx); + + // measure absolute velocity + Eigen::MatrixXd Vx = Eigen::MatrixXd::Zero(1, 1); + Vx << object.kinematics.twist_with_covariance.twist.linear.x; + + Eigen::Matrix2d R_vx = Eigen::MatrixXd::Zero(1, 1); + if (!object.kinematics.has_twist_covariance) { + R_vx << ekf_params_.r_cov_vx; + } else { + R_vx << object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + } + R_block_list.push_back(R_vx); + } + + // 4. sum up matrices + const auto row_number = std::accumulate( + C_list.begin(), C_list.end(), 0, + [](const auto & sum, const auto & C) { return sum + C.rows(); }); + if (row_number == 0) { + RCLCPP_WARN(logger_, "No measurement is available"); + return false; + } + + // stacking matrices vertically or diagonally + const auto C = utils::stackMatricesVertically(C_list); + const auto Y = utils::stackMatricesVertically(Y_list); + const auto R = utils::stackMatricesDiagonally(R_block_list); + + // 4. EKF update + if (!ekf_.update(Y, C, R)) { + RCLCPP_WARN(logger_, "Cannot update"); + } + + // 5. normalize: limit vx + { + 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_; + } + ekf_.init(X_t, P_t); + } + + // 6. Filter z + // first order low pass filter + const float gain = filter_tau_ / (filter_tau_ + filter_dt_); + z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + return true; +} + +bool ConstantTurnRateMotionTracker::measureWithShape( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + // just use first order low pass filter + const float gain = filter_tau_ / (filter_tau_ + filter_dt_); + + 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; + } 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; + } else { + return false; + } + + return true; +} + +bool ConstantTurnRateMotionTracker::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(); + object_ = object; + if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + setClassification(current_classification); + } + + if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + RCLCPP_WARN( + logger_, "There is a large gap between predicted time and measurement time. (%f)", + (time - last_update_time_).seconds()); + } + + measureWithPose(object, self_transform); + measureWithShape(object); + + // (void)self_transform; // currently do not use self vehicle position + return true; +} + +bool ConstantTurnRateMotionTracker::getTrackedObject( + const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const +{ + object = object_recognition_utils::toTrackedObject(object_); + 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; + // rotation matrix with yaw_ + Eigen::Matrix2d R_yaw = Eigen::Matrix2d::Zero(); + R_yaw << std::cos(X_t(IDX::YAW)), -std::sin(X_t(IDX::YAW)), std::sin(X_t(IDX::YAW)), + std::cos(X_t(IDX::YAW)); + const Eigen::Matrix2d R_yaw_inv = R_yaw.transpose(); + + // 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(); + if (trust_yaw_input_) { + object.kinematics.orientation_availability = + autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; + } else { + object.kinematics.orientation_availability = + autoware_auto_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE; + } + } + // twist + // twist need to converted to local coordinate + const auto vx = X_t(IDX::VX); + twist_with_cov.twist.linear.x = vx; + + // ===== covariance transformation ===== + // since covariance in EKF is in map coordinate and output should be in object coordinate, + // we need to transform covariance + Eigen::Matrix2d P_xy_map; + P_xy_map << P(IDX::X, IDX::X), P(IDX::X, IDX::Y), P(IDX::Y, IDX::X), P(IDX::Y, IDX::Y); + + // rotate covariance with -yaw + Eigen::Matrix2d P_xy = R_yaw_inv * P_xy_map * R_yaw_inv.transpose(); + + // position covariance + constexpr double no_info_cov = 1e9; // no information + constexpr double z_cov = 1. * 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_xy(IDX::X, IDX::X); + pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_xy(IDX::X, IDX::Y); + pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_xy(IDX::Y, IDX::X); + pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_xy(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] = no_info_cov; + pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; + pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = yaw_cov; + + // twist covariance + constexpr double vz_cov = 0.2 * 0.2; // TODO(Yoshi Ri) Currently tentative + constexpr double wz_cov = 0.2 * 0.2; // 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::X_Y] = 0.0; + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = 0.0; + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = no_info_cov; + twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; + twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; + twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = wz_cov; + + // set shape + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + object.shape.dimensions.x = bounding_box_.length; + object.shape.dimensions.y = bounding_box_.width; + object.shape.dimensions.z = bounding_box_.height; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + object.shape.dimensions.x = cylinder_.width; + object.shape.dimensions.y = cylinder_.width; + object.shape.dimensions.z = cylinder_.height; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); + const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); + object.shape.footprint = + tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + } + + return true; +} diff --git a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp index f6814ceb8c246..8369d04fbe225 100644 --- a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp +++ b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp @@ -44,9 +44,21 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; +// initialize static parameter +bool LinearMotionTracker::is_initialized_ = false; +LinearMotionTracker::EkfParams LinearMotionTracker::ekf_params_; +double LinearMotionTracker::max_vx_; +double LinearMotionTracker::max_vy_; +double LinearMotionTracker::filter_tau_; +double LinearMotionTracker::filter_dt_; +bool LinearMotionTracker::estimate_acc_; +bool LinearMotionTracker::trust_yaw_input_; +bool LinearMotionTracker::trust_twist_input_; +bool LinearMotionTracker::use_polar_coordinate_in_measurement_noise_; + LinearMotionTracker::LinearMotionTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const std::string & /*tracker_param_file*/, const std::uint8_t & /*label*/) + const std::string & tracker_param_file, const std::uint8_t & /*label*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("LinearMotionTracker")), last_update_time_(time), @@ -55,11 +67,13 @@ LinearMotionTracker::LinearMotionTracker( object_ = object; // load setting from yaml file - const std::string default_setting_file = - ament_index_cpp::get_package_share_directory("radar_object_tracker") + - "/config/tracking/linear_motion_tracker.yaml"; - loadDefaultModelParameters(default_setting_file); - // loadModelParameters(tracker_param_file, label); + if (!is_initialized_) { + loadDefaultModelParameters(tracker_param_file); + is_initialized_ = true; + } + // shape initialization + bounding_box_ = {0.5, 0.5, 1.7}; + cylinder_ = {0.3, 1.7}; // initialize X matrix and position Eigen::MatrixXd X(ekf_params_.dim_x, 1); @@ -193,6 +207,12 @@ void LinearMotionTracker::loadDefaultModelParameters(const std::string & path) config["default"]["ekf_params"]["initial_covariance_std"]["ax"].as(); // [m/(s*s)] const float p0_stddev_ay = config["default"]["ekf_params"]["initial_covariance_std"]["ay"].as(); // [m/(s*s)] + estimate_acc_ = config["default"]["ekf_params"]["estimate_acc"].as(); + trust_yaw_input_ = config["default"]["trust_yaw_input"].as(false); // default false + trust_twist_input_ = config["default"]["trust_twist_input"].as(false); // default false + use_polar_coordinate_in_measurement_noise_ = + config["default"]["use_polar_coordinate_in_measurement_noise"].as( + false); // default false ekf_params_.q_cov_ax = std::pow(q_stddev_ax, 2.0); ekf_params_.q_cov_ay = std::pow(q_stddev_ay, 2.0); ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); @@ -219,11 +239,6 @@ void LinearMotionTracker::loadDefaultModelParameters(const std::string & path) const float max_speed_kmph = config["default"]["limit"]["max_speed"].as(); // [km/h] max_vx_ = tier4_autoware_utils::kmph2mps(max_speed_kmph); // [m/s] max_vy_ = tier4_autoware_utils::kmph2mps(max_speed_kmph); // [rad/s] - - // shape initialization - // (TODO): this may be written in another yaml file based on classify result - bounding_box_ = {0.5, 0.5, 1.7}; - cylinder_ = {0.3, 1.7}; } bool LinearMotionTracker::predict(const rclcpp::Time & time) @@ -257,6 +272,8 @@ bool LinearMotionTracker::predict(const double dt, KalmanFilter & ekf) const * 0, 0, 0, 0, 1, 0, * 0, 0, 0, 0, 0, 1] */ + // estimate acc + const double acc_coeff = estimate_acc_ ? 1.0 : 0.0; // X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state @@ -270,10 +287,10 @@ bool LinearMotionTracker::predict(const double dt, KalmanFilter & ekf) const // X t+1 Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); - X_next_t(IDX::X) = x + vx * dt + 0.5 * ax * dt * dt; - X_next_t(IDX::Y) = y + vy * dt + 0.5 * ay * dt * dt; - X_next_t(IDX::VX) = vx + ax * dt; - X_next_t(IDX::VY) = vy + ay * dt; + X_next_t(IDX::X) = x + vx * dt + 0.5 * ax * dt * dt * acc_coeff; + X_next_t(IDX::Y) = y + vy * dt + 0.5 * ay * dt * dt * acc_coeff; + X_next_t(IDX::VX) = vx + ax * dt * acc_coeff; + X_next_t(IDX::VY) = vy + ay * dt * acc_coeff; X_next_t(IDX::AX) = ax; X_next_t(IDX::AY) = ay; @@ -281,10 +298,10 @@ bool LinearMotionTracker::predict(const double dt, KalmanFilter & ekf) const 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; - A(IDX::X, IDX::AX) = 0.5 * dt * dt; - A(IDX::Y, IDX::AY) = 0.5 * dt * dt; - A(IDX::VX, IDX::AX) = dt; - A(IDX::VY, IDX::AY) = dt; + A(IDX::X, IDX::AX) = 0.5 * dt * dt * acc_coeff; + A(IDX::Y, IDX::AY) = 0.5 * dt * dt * acc_coeff; + A(IDX::VX, IDX::AX) = dt * acc_coeff; + A(IDX::VY, IDX::AY) = dt * acc_coeff; // Q: system noise Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); @@ -333,7 +350,8 @@ bool LinearMotionTracker::predict(const double dt, KalmanFilter & ekf) const } bool LinearMotionTracker::measureWithPose( - const autoware_auto_perception_msgs::msg::DetectedObject & object) + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) { // Observation pattern will be: // 1. x, y, vx, vy @@ -348,8 +366,25 @@ bool LinearMotionTracker::measureWithPose( // rotation matrix Eigen::Matrix2d RotationYaw; - const auto yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - RotationYaw << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); + if (trust_yaw_input_) { + const auto yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + RotationYaw << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); + } else { + RotationYaw << std::cos(yaw_), -std::sin(yaw_), std::sin(yaw_), std::cos(yaw_); + } + const auto base_link_yaw = tf2::getYaw(self_transform.rotation); + Eigen::Matrix2d RotationBaseLink; + RotationBaseLink << std::cos(base_link_yaw), -std::sin(base_link_yaw), std::sin(base_link_yaw), + std::cos(base_link_yaw); + + // depth from base_link to object + const auto dx = + object.kinematics.pose_with_covariance.pose.position.x - self_transform.translation.x; + const auto dy = + object.kinematics.pose_with_covariance.pose.position.y - self_transform.translation.y; + Eigen::Vector2d pose_diff_in_map(dx, dy); + Eigen::Vector2d pose_diff_in_base_link = RotationBaseLink * pose_diff_in_map; + const auto depth = abs(pose_diff_in_base_link(0)); // gather matrices as vector std::vector C_list; @@ -371,21 +406,27 @@ bool LinearMotionTracker::measureWithPose( // covariance need to be rotated since it is in the vehicle coordinate system Eigen::MatrixXd Rxy_local = Eigen::MatrixXd::Zero(2, 2); + Eigen::MatrixXd Rxy = Eigen::MatrixXd::Zero(2, 2); if (!object.kinematics.has_position_covariance) { - Rxy_local << ekf_params_.r_cov_x, 0, 0, ekf_params_.r_cov_y; + // switch noise covariance in polar coordinate or cartesian coordinate + const auto r_cov_y = use_polar_coordinate_in_measurement_noise_ + ? depth * depth * ekf_params_.r_cov_x + : ekf_params_.r_cov_y; + Rxy_local << ekf_params_.r_cov_x, 0, 0, r_cov_y; // xy in base_link coordinate + Rxy = RotationBaseLink * Rxy_local * RotationBaseLink.transpose(); } else { Rxy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X], object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y], object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + object.kinematics.pose_with_covariance + .covariance[utils::MSG_COV_IDX::Y_Y]; // xy in vehicle coordinate + Rxy = RotationYaw * Rxy_local * RotationYaw.transpose(); } - Eigen::MatrixXd Rxy = Eigen::MatrixXd::Zero(2, 2); - Rxy = RotationYaw * Rxy_local * RotationYaw.transpose(); R_block_list.push_back(Rxy); } // 2. add linear velocity measurement - const bool enable_velocity_measurement = object.kinematics.has_twist; + const bool enable_velocity_measurement = object.kinematics.has_twist && trust_twist_input_; if (enable_velocity_measurement) { Eigen::MatrixXd C_vx_vy = Eigen::MatrixXd::Zero(2, ekf_params_.dim_x); C_vx_vy(0, IDX::VX) = 1; @@ -401,14 +442,15 @@ bool LinearMotionTracker::measureWithPose( Y_list.push_back(Vxy); Eigen::Matrix2d R_v_xy_local = Eigen::MatrixXd::Zero(2, 2); + Eigen::MatrixXd R_v_xy = Eigen::MatrixXd::Zero(2, 2); if (!object.kinematics.has_twist_covariance) { R_v_xy_local << ekf_params_.r_cov_vx, 0, 0, ekf_params_.r_cov_vy; + R_v_xy = RotationBaseLink * R_v_xy_local * RotationBaseLink.transpose(); } else { R_v_xy_local << object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X], 0, 0, object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + R_v_xy = RotationYaw * R_v_xy_local * RotationYaw.transpose(); } - Eigen::MatrixXd R_v_xy = Eigen::MatrixXd::Zero(2, 2); - R_v_xy = RotationYaw * R_v_xy_local * RotationYaw.transpose(); R_block_list.push_back(R_v_xy); } @@ -453,8 +495,16 @@ bool LinearMotionTracker::measureWithPose( // first order low pass filter const float gain = filter_tau_ / (filter_tau_ + filter_dt_); z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; - yaw_ = gain * yaw_ + (1.0 - gain) * yaw; - + // get yaw from twist atan + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + ekf_.getX(X_t); + const auto twist_yaw = + std::atan2(X_t(IDX::VY), X_t(IDX::VX)); // calc from lateral and longitudinal velocity + if (trust_yaw_input_) { + yaw_ = gain * yaw_ + (1.0 - gain) * twist_yaw; + } else { + yaw_ = twist_yaw; + } return true; } @@ -494,10 +544,10 @@ bool LinearMotionTracker::measure( (time - last_update_time_).seconds()); } - measureWithPose(object); + measureWithPose(object, self_transform); measureWithShape(object); - (void)self_transform; // currently do not use self vehicle position + // (void)self_transform; // currently do not use self vehicle position return true; } @@ -543,8 +593,13 @@ bool LinearMotionTracker::getTrackedObject( 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; + if (trust_yaw_input_) { + object.kinematics.orientation_availability = + autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; + } else { + object.kinematics.orientation_availability = + autoware_auto_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE; + } } // twist // twist need to converted to local coordinate diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp index 1122bf7b4b18e..d8b5599cc1ca7 100644 --- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp +++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp @@ -244,6 +244,11 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() std::atan2(compensated_velocity.y, compensated_velocity.x)); geometry_msgs::msg::PoseStamped transformed_pose_stamped{}; + if (transform_ == nullptr) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 5000, "getTransform failed. radar output will be empty."); + return tracked_objects; + } tf2::doTransform(radar_pose_stamped, transformed_pose_stamped, *transform_); kinematics.pose_with_covariance.pose = transformed_pose_stamped.pose; kinematics.pose_with_covariance.pose.orientation = diff --git a/perception/tracking_object_merger/CMakeLists.txt b/perception/tracking_object_merger/CMakeLists.txt index 108749c5f91a7..ed5aa76afbfd9 100644 --- a/perception/tracking_object_merger/CMakeLists.txt +++ b/perception/tracking_object_merger/CMakeLists.txt @@ -7,7 +7,6 @@ endif() find_package(autoware_cmake REQUIRED) autoware_package() -find_package(nlohmann_json REQUIRED) # for debug # find dependencies @@ -36,7 +35,6 @@ ament_auto_add_library(decorative_tracker_merger_node SHARED target_link_libraries(decorative_tracker_merger_node mu_successive_shortest_path Eigen3::Eigen - nlohmann_json::nlohmann_json # for debug ) rclcpp_components_register_node(decorative_tracker_merger_node diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp index 05fc9f6caccb5..a62a39d11c192 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp @@ -19,8 +19,6 @@ #ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ #define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ -// #include // for debug json library - #include #include #include diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index ee6e50f5a9553..e71d1dd9d86b4 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -30,6 +30,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/turn_signal_decider.cpp src/utils/utils.cpp src/utils/path_utils.cpp + src/utils/traffic_light_utils.cpp src/utils/path_safety_checker/safety_check.cpp src/utils/path_safety_checker/objects_filtering.cpp src/utils/start_goal_planner_common/utils.cpp diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 25e42d20d144a..96b6ca452bbcc 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -9,7 +9,6 @@ # avoidance module common setting enable_bound_clipping: false - enable_force_avoidance_for_stopped_vehicle: false enable_yield_maneuver: true enable_yield_maneuver_during_shifting: false enable_cancel_maneuver: false @@ -37,6 +36,7 @@ avoid_margin_lateral: 1.0 # [m] safety_buffer_lateral: 0.7 # [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: is_target: true execute_num: 1 @@ -47,6 +47,7 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 + use_conservative_buffer_longitudinal: true bus: is_target: true execute_num: 1 @@ -57,6 +58,7 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 + use_conservative_buffer_longitudinal: true trailer: is_target: true execute_num: 1 @@ -67,6 +69,7 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 + use_conservative_buffer_longitudinal: true unknown: is_target: false execute_num: 1 @@ -77,6 +80,7 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 + use_conservative_buffer_longitudinal: true bicycle: is_target: false execute_num: 1 @@ -87,6 +91,7 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 + use_conservative_buffer_longitudinal: true motorcycle: is_target: false execute_num: 1 @@ -97,6 +102,7 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 + use_conservative_buffer_longitudinal: true pedestrian: is_target: false execute_num: 1 @@ -107,16 +113,12 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 + use_conservative_buffer_longitudinal: true lower_distance_for_polygon_expansion: 30.0 # [m] upper_distance_for_polygon_expansion: 100.0 # [m] # For target object filtering target_filtering: - # params for avoidance of not-parked objects - threshold_time_force_avoidance_for_stopped_vehicle: 10.0 # [s] - object_ignore_section_traffic_light_in_front_distance: 30.0 # [m] - object_ignore_section_crosswalk_in_front_distance: 30.0 # [m] - object_ignore_section_crosswalk_behind_distance: 30.0 # [m] # detection range object_check_goal_distance: 20.0 # [m] # filtering parking objects @@ -133,6 +135,17 @@ max_forward_distance: 150.0 # [m] backward_distance: 10.0 # [m] + # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. + force_avoidance: + enable: false # [-] + time_threshold: 1.0 # [s] + ignore_area: + traffic_light: + front_distance: 100.0 # [m] + crosswalk: + front_distance: 30.0 # [m] + behind_distance: 30.0 # [m] + # For safety check safety_check: # safety check configuration @@ -178,11 +191,18 @@ # avoidance distance parameters longitudinal: prepare_time: 2.0 # [s] - remain_buffer_distance: 30.0 # [m] min_prepare_distance: 1.0 # [m] min_slow_down_speed: 1.38 # [m/s] buf_slow_down_speed: 0.56 # [m/s] nominal_avoidance_speed: 8.33 # [m/s] + # return dead line + return_dead_line: + goal: + enable: true # [-] + buffer: 30.0 # [m] + traffic_light: + enable: true # [-] + buffer: 3.0 # [m] # For yield maneuver yield: diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index d333cbfd778cb..b4d323de45cde 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -2,6 +2,7 @@ ros__parameters: verbose: false max_iteration_num: 100 + traffic_light_signal_timeout: 1.0 planning_hz: 10.0 backward_path_length: 5.0 forward_path_length: 300.0 diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index d3c0a22e867f9..f98eac5315006 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -5,6 +5,7 @@ prepare_duration: 4.0 # [s] backward_length_buffer_for_end_of_lane: 3.0 # [m] + backward_length_buffer_for_blocking_object: 3.0 # [m] lane_change_finish_judge_buffer: 2.0 # [m] lane_changing_lateral_jerk: 0.5 # [m/s3] 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 420a5a8aa6ee5..1a9b23be00a3f 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 @@ -58,6 +58,7 @@ using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_perception_msgs::msg::TrafficSignalArray; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::PoseWithUuidStamped; using nav_msgs::msg::OccupancyGrid; @@ -93,6 +94,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr perception_subscriber_; rclcpp::Subscription::SharedPtr occupancy_grid_subscriber_; rclcpp::Subscription::SharedPtr costmap_subscriber_; + rclcpp::Subscription::SharedPtr traffic_signals_subscriber_; rclcpp::Subscription::SharedPtr lateral_offset_subscriber_; rclcpp::Subscription::SharedPtr operation_mode_subscriber_; rclcpp::Publisher::SharedPtr path_publisher_; @@ -136,6 +138,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node void onPerception(const PredictedObjects::ConstSharedPtr msg); void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg); void onCostMap(const OccupancyGrid::ConstSharedPtr msg); + void onTrafficSignals(const TrafficSignalArray::ConstSharedPtr msg); void onMap(const HADMapBin::ConstSharedPtr map_msg); void onRoute(const LaneletRoute::ConstSharedPtr route_msg); void onOperationMode(const OperationModeState::ConstSharedPtr msg); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 9280a81e643ca..ca58144464731 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -20,6 +20,7 @@ #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" #include "motion_utils/trajectory/trajectory.hpp" +#include #include #include @@ -28,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -39,6 +41,7 @@ #include #include +#include #include #include #include @@ -51,6 +54,7 @@ using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_perception_msgs::msg::TrafficSignal; using autoware_planning_msgs::msg::PoseWithUuidStamped; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::PoseStamped; @@ -59,8 +63,15 @@ using nav_msgs::msg::Odometry; using route_handler::RouteHandler; using tier4_planning_msgs::msg::LateralOffset; using PlanResult = PathWithLaneId::SharedPtr; +using lanelet::TrafficLight; using unique_identifier_msgs::msg::UUID; +struct TrafficSignalStamped +{ + builtin_interfaces::msg::Time stamp; + TrafficSignal signal; +}; + struct BoolStamped { explicit BoolStamped(bool in_data) : data(in_data) {} @@ -145,6 +156,7 @@ struct PlannerData std::optional prev_modified_goal{}; std::optional prev_route_id{}; std::shared_ptr route_handler{std::make_shared()}; + std::map traffic_light_id_map; BehaviorPathPlannerParameters parameters{}; drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{}; @@ -162,6 +174,21 @@ struct PlannerData route_handler, path, turn_signal_info, current_pose, current_vel, parameters, debug_data); } + std::optional getTrafficSignal(const int id) const + { + if (traffic_light_id_map.count(id) == 0) { + return std::nullopt; + } + + const auto elapsed_time = + (rclcpp::Clock{RCL_ROS_TIME}.now() - traffic_light_id_map.at(id).stamp).seconds(); + if (elapsed_time > parameters.traffic_light_signal_timeout) { + return std::nullopt; + } + + return traffic_light_id_map.at(id); + } + template size_t findEgoIndex(const std::vector & points) const { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index a4e6fc18ab050..1998e7b99dd65 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -77,6 +77,7 @@ struct BehaviorPathPlannerParameters { bool verbose; size_t max_iteration_num{100}; + double traffic_light_signal_timeout{1.0}; ModuleConfigParameters config_avoidance; ModuleConfigParameters config_avoidance_by_lc; @@ -92,6 +93,7 @@ struct BehaviorPathPlannerParameters double backward_path_length; double forward_path_length; double backward_length_buffer_for_end_of_lane; + double backward_length_buffer_for_blocking_object; double backward_length_buffer_for_end_of_pull_over; double backward_length_buffer_for_end_of_pull_out; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 343302a022a69..df21bad862cc9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -135,13 +135,12 @@ class PullOverStatus DEFINE_SETTER_GETTER(bool, has_decided_velocity) DEFINE_SETTER_GETTER(bool, has_requested_approval) DEFINE_SETTER_GETTER(bool, is_ready) - DEFINE_SETTER_GETTER(std::shared_ptr, last_approved_time) DEFINE_SETTER_GETTER(std::shared_ptr, last_increment_time) DEFINE_SETTER_GETTER(std::shared_ptr, last_path_update_time) - DEFINE_SETTER_GETTER(std::shared_ptr, last_approved_pose) DEFINE_SETTER_GETTER(std::optional, modified_goal_pose) DEFINE_SETTER_GETTER(Pose, refined_goal_pose) DEFINE_SETTER_GETTER(GoalCandidates, goal_candidates) + DEFINE_SETTER_GETTER(Pose, closest_goal_candidate_pose) DEFINE_SETTER_GETTER(std::vector, pull_over_path_candidates) DEFINE_SETTER_GETTER(std::optional, closest_start_pose) @@ -165,15 +164,14 @@ class PullOverStatus bool is_ready_{false}; // save last time and pose - std::shared_ptr last_approved_time_; std::shared_ptr last_increment_time_; std::shared_ptr last_path_update_time_; - std::shared_ptr last_approved_pose_; // goal modification std::optional modified_goal_pose_; Pose refined_goal_pose_{}; GoalCandidates goal_candidates_{}; + Pose closest_goal_candidate_pose_{}; // pull over path std::vector pull_over_path_candidates_; @@ -197,6 +195,14 @@ struct GoalPlannerDebugData std::vector ego_polygons_expanded{}; }; +struct LastApprovalData +{ + LastApprovalData(rclcpp::Time time, Pose pose) : time(time), pose(pose) {} + + rclcpp::Time time{}; + Pose pose{}; +}; + class GoalPlannerModule : public SceneModuleInterface { public: @@ -217,31 +223,28 @@ class GoalPlannerModule : public SceneModuleInterface } } - // TODO(someone): remove this, and use base class function - [[deprecated]] BehaviorModuleOutput run() override; - bool isExecutionRequested() const override; - bool isExecutionReady() const override; - // TODO(someone): remove this, and use base class function - [[deprecated]] ModuleStatus updateState() override; + CandidateOutput planCandidate() const override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; - void processOnEntry() override; + bool isExecutionRequested() const override; + bool isExecutionReady() const override; void processOnExit() override; + void updateData() override; void setParameters(const std::shared_ptr & parameters); void acceptVisitor( [[maybe_unused]] const std::shared_ptr & visitor) const override { } - // not used, but need to override - CandidateOutput planCandidate() const override { return CandidateOutput{}; }; - private: + // The start_planner activates when it receives a new route, + // so there is no need to terminate the goal planner. + // If terminating it, it may switch to lane following and could generate an inappropriate path. bool canTransitSuccessState() override { return false; } bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return false; } + bool canTransitIdleToRunningState() override { return true; } mutable StartGoalPlannerData goal_planner_data_; @@ -274,6 +277,8 @@ class GoalPlannerModule : public SceneModuleInterface std::recursive_mutex mutex_; PullOverStatus status_; + std::unique_ptr last_approval_data_{nullptr}; + // approximate distance from the start point to the end point of pull_over. // this is used as an assumed value to decelerate, etc., before generating the actual path. const double approximate_pull_over_distance_{20.0}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 472564a061f04..daad5b1a40610 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -107,7 +107,7 @@ class LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const double threshold) const = 0; - virtual bool getAbortPath() = 0; + virtual bool calcAbortPath() = 0; virtual bool specialRequiredCheck() const { return false; } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 54883489f2fe6..503542be7cda6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -72,7 +72,7 @@ class NormalLaneChange : public LaneChangeBase TurnSignalInfo updateOutputTurnSignal() override; - bool getAbortPath() override; + bool calcAbortPath() override; PathSafetyStatus isApprovedPathSafe() const override; @@ -155,6 +155,9 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes) const; + std::vector filterObjectsInTargetLane( + const LaneChangeTargetObjects & objects, const lanelet::ConstLanelets & target_lanes) const; + //! @brief Check if the ego vehicle is in stuck by a stationary obstacle. //! @param obstacle_check_distance Distance to check ahead for any objects that might be //! obstructing ego path. It makes sense to use values like the maximum lane change distance. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index 7f24683e5680c..534594d93445c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -289,7 +289,7 @@ class SceneModuleInterface return calcOffsetPose(dead_pose_.get(), base_link2front, 0.0, 0.0); } - void resetWallPoses() + void resetWallPoses() const { stop_pose_ = boost::none; slow_pose_ = boost::none; @@ -477,7 +477,7 @@ class SceneModuleInterface * @brief Return true if the activation command is received from the RTC interface. * If no RTC interface is registered, return true. */ - bool isActivated() + bool isActivated() const { if (rtc_interface_ptr_map_.empty()) { return true; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 7ab404d99bb12..8ea963fec0b8d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -51,6 +51,7 @@ using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using geometry_msgs::msg::PoseArray; using lane_departure_checker::LaneDepartureChecker; +using PriorityOrder = std::vector>>; struct PullOutStatus { @@ -59,10 +60,9 @@ struct PullOutStatus PlannerType planner_type{PlannerType::NONE}; PathWithLaneId backward_path{}; lanelet::ConstLanelets pull_out_lanes{}; - bool is_safe_static_objects{false}; // current path is safe against static objects + bool found_pull_out_path{false}; // current path is safe against static objects bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects - bool back_finished{false}; // if backward driving is not required, this is also set to true - // todo: rename to clear variable name. + bool driving_forward{false}; // if ego is driving on backward path, this is set to false bool backward_driving_complete{ false}; // after backward driving is complete, this is set to true (warning: this is set to // false at next cycle after backward driving is complete) @@ -120,7 +120,7 @@ class StartPlannerModule : public SceneModuleInterface } // Condition to disable simultaneous execution - bool isBackFinished() const { return status_.back_finished; } + bool isDrivingForward() const { return status_.driving_forward; } bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; } private: @@ -132,6 +132,26 @@ class StartPlannerModule : public SceneModuleInterface void initializeSafetyCheckParameters(); + bool isModuleRunning() const; + bool isCurrentPoseOnMiddleOfTheRoad() const; + bool isCloseToOriginalStartPose() const; + bool hasArrivedAtGoal() const; + bool isMoving() const; + + PriorityOrder determinePriorityOrder( + const std::string & search_priority, const size_t candidates_size); + bool findPullOutPath( + const std::vector & start_pose_candidates, const size_t index, + const std::shared_ptr & planner, const Pose & refined_start_pose, + const Pose & goal_pose); + void updateStatusWithCurrentPath( + const behavior_path_planner::PullOutPath & path, const Pose & start_pose, + const behavior_path_planner::PlannerType & planner_type); + void updateStatusWithNextPath( + const behavior_path_planner::PullOutPath & path, const Pose & start_pose, + const behavior_path_planner::PlannerType & planner_type); + void updateStatusIfNoSafePathFound(); + std::shared_ptr parameters_; mutable std::shared_ptr ego_predicted_path_params_; mutable std::shared_ptr objects_filtering_params_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 74464be0fa5ea..c8b87c7b4759f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -71,6 +71,8 @@ struct ObjectParameter double safety_buffer_lateral{1.0}; double safety_buffer_longitudinal{0.0}; + + bool use_conservative_buffer_longitudinal{true}; }; struct AvoidanceParameters @@ -110,6 +112,14 @@ struct AvoidanceParameters // use intersection area for avoidance bool use_intersection_areas{false}; + // consider avoidance return dead line + bool enable_dead_line_for_goal{false}; + bool enable_dead_line_for_traffic_light{false}; + + // module try to return original path to keep this distance from edge point of the path. + double dead_line_buffer_for_goal{0.0}; + double dead_line_buffer_for_traffic_light{0.0}; + // max deceleration for double max_deceleration{0.0}; @@ -215,9 +225,6 @@ struct AvoidanceParameters // nominal avoidance sped double nominal_avoidance_speed{0.0}; - // module try to return original path to keep this distance from edge point of the path. - double remain_buffer_distance{0.0}; - // The margin is configured so that the generated avoidance trajectory does not come near to the // road shoulder. double soft_road_shoulder_margin{1.0}; @@ -379,6 +386,9 @@ struct ObjectData // avoidance target // is stoppable under the constraints bool is_stoppable{false}; + // is within intersection area + bool is_within_intersection{false}; + // unavoidable reason std::string reason{""}; @@ -512,6 +522,10 @@ struct AvoidancePlanningData bool found_avoidance_path{false}; double to_stop_line{std::numeric_limits::max()}; + + double to_start_point{std::numeric_limits::lowest()}; + + double to_return_point{std::numeric_limits::max()}; }; /* diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 0237fb458ea0b..2e04935e37336 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -39,6 +39,9 @@ bool isWithinCrosswalk( const ObjectData & object, const std::shared_ptr & overall_graphs); +bool isWithinIntersection( + const ObjectData & object, const std::shared_ptr & route_handler); + bool isTargetObjectType( const PredictedObject & object, const std::shared_ptr & parameters); @@ -169,6 +172,16 @@ std::pair separateObjectsByPath( DrivableLanes generateExpandDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, const std::shared_ptr & parameters); + +double calcDistanceToReturnDeadLine( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); + +double calcDistanceToAvoidStartLine( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); } // namespace behavior_path_planner::utils::avoidance #endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp index 2ed58b9678e70..2fc0acf1c5086 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp @@ -35,6 +35,8 @@ class GoalSearcher : public GoalSearcherBase GoalCandidates search() override; void update(GoalCandidates & goal_candidates) const override; + GoalCandidate getClosetGoalCandidateAlongLanes( + const GoalCandidates & goal_candidates) const override; private: void countObjectsToAvoid( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp index ab319111f6da6..24c614e072b8f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp @@ -58,6 +58,8 @@ class GoalSearcherBase MultiPolygon2d getAreaPolygons() { return area_polygons_; } virtual GoalCandidates search() = 0; virtual void update([[maybe_unused]] GoalCandidates & goal_candidates) const { return; } + virtual GoalCandidate getClosetGoalCandidateAlongLanes( + const GoalCandidates & goal_candidates) const = 0; protected: GoalPlannerParameters parameters_{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index dc113b0b0be18..3634586c40540 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -75,6 +75,7 @@ struct ExtendedPredictedObject geometry_msgs::msg::TwistWithCovariance initial_twist; geometry_msgs::msg::AccelWithCovariance initial_acceleration; autoware_auto_perception_msgs::msg::Shape shape; + std::vector classification; std::vector predicted_paths; }; using ExtendedPredictedObjects = std::vector; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 8409116236169..b8bd1629d5f3f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -107,10 +107,6 @@ bool checkCollision( const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, const double hysteresis_factor, CollisionCheckDebug & debug); -std::vector generatePolygonsWithStoppingAndInertialMargin( - const PathWithLaneId & ego_path, const double base_to_front, const double base_to_rear, - const double width, const double maximum_deceleration, const double max_extra_stopping_margin); - /** * @brief Iterate the points in the ego and target's predicted path and * perform safety check for each of the iterated points. @@ -133,13 +129,9 @@ std::vector getCollidedPolygons( const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, const double hysteresis_factor, CollisionCheckDebug & debug); -/** - * @brief Check collision between ego polygons with margin and objects. - * @return Has collision or not - */ -bool checkCollisionWithMargin( - const std::vector & ego_polygons, const PredictedObjects & dynamic_objects, - const double collision_check_margin); +bool checkPolygonsIntersects( + const std::vector & polys_1, const std::vector & polys_2); + } // namespace behavior_path_planner::utils::path_safety_checker #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp index b42502d9edec8..a9cbf859dd9d8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp @@ -104,6 +104,8 @@ PathWithLaneId calcCenterLinePath( const std::optional & prev_module_path = std::nullopt); PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & path2); + +boost::optional getFirstStopPoseFromPath(const PathWithLaneId & path); } // namespace behavior_path_planner::utils #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/traffic_light_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/traffic_light_utils.hpp new file mode 100644 index 0000000000000..aa3e7f4833134 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/traffic_light_utils.hpp @@ -0,0 +1,112 @@ +// Copyright 2023 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 BEHAVIOR_PATH_PLANNER__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include + +namespace behavior_path_planner::utils::traffic_light +{ + +using autoware_perception_msgs::msg::TrafficSignal; +using autoware_perception_msgs::msg::TrafficSignalElement; +using geometry_msgs::msg::Pose; + +/** + * @brief Checks if a traffic light state includes a circle-shaped light with the specified color. + * + * Iterates through the traffic light elements to find a circle-shaped light that matches the given + * color. + * + * @param tl_state The traffic light state to check. + * @param lamp_color The color to look for in the traffic light's circle-shaped lamps. + * @return True if a circle-shaped light with the specified color is found, false otherwise. + */ +bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color); + +/** + * @brief Checks if a traffic light state includes a light with the specified shape. + * + * Searches through the traffic light elements to find a light that matches the given shape. + * + * @param tl_state The traffic light state to check. + * @param shape The shape to look for in the traffic light's lights. + * @return True if a light with the specified shape is found, false otherwise. + */ +bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape); + +/** + * @brief Determines if a traffic signal indicates a stop for the given lanelet. + * + * Evaluates the current state of the traffic light, considering if it's green or unknown, + * which would not necessitate a stop. Then, it checks the turn direction attribute of the lanelet + * against the traffic light's arrow shapes to determine whether a vehicle must stop or if it can + * proceed based on allowed turn directions. + * + * @param lanelet The lanelet to check for a stop signal at its traffic light. + * @param tl_state The current state of the traffic light associated with the lanelet. + * @return True if the traffic signal indicates a stop is required, false otherwise. + */ +bool isTrafficSignalStop(const lanelet::ConstLanelet & lanelet, const TrafficSignal & tl_state); + +/** + * @brief Computes the distance from the current position to the next traffic light along a set of + * lanelets. + * + * This function finds the closest lanelet to the current position and then searches for the + * next traffic light within the lanelet sequence. If a traffic light is found, it calculates + * the distance to the stop line of that traffic light from the current position. + * + * @param current_pose The current position of ego vehicle. + * @param lanelets A collection of lanelets representing the road ahead. + * @return The distance to the next traffic light's stop line or infinity if no traffic light is + * found ahead. + */ +double getDistanceToNextTrafficLight( + const Pose & current_pose, const lanelet::ConstLanelets & lanelets); + +/** + * @brief Calculates the signed distance from the ego vehicle to the stop line of the nearest red + * traffic light. + * + * Iterates over lanelets to find traffic lights. If a red traffic light is found, + * computes the distance to its stop line from the ego vehicle's position along a specified path. + * + * @param lanelets Collection of lanelets to inspect for traffic light regulatory elements. + * @param path The path along which the distance from ego current position to the stop line is + * measured. + * @param planner_data Shared pointer to planner data with vehicle odometry and traffic signal + * information. + * @return Optional double value with the signed distance to the stop line, or std::nullopt if no + * red traffic light is detected. + */ +std::optional calcDistanceToRedTrafficLight( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data); +} // namespace behavior_path_planner::utils::traffic_light + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index f418c9c7300f7..3f9591f6c2143 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -130,9 +130,6 @@ double l2Norm(const Vector3 vector); double getDistanceToEndOfLane(const Pose & current_pose, const lanelet::ConstLanelets & lanelets); -double getDistanceToNextTrafficLight( - const Pose & current_pose, const lanelet::ConstLanelets & lanelets); - double getDistanceToNextIntersection( const Pose & current_pose, const lanelet::ConstLanelets & lanelets); @@ -330,6 +327,8 @@ std::optional getSignedDistanceFromBoundary( Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet); +Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon); + std::vector getTargetLaneletPolygons( const lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length, const std::string & target_type); @@ -365,10 +364,12 @@ lanelet::ConstLanelets getCurrentLanesFromPath( const PathWithLaneId & path, const std::shared_ptr & planner_data); lanelet::ConstLanelets extendNextLane( - const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes); + const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes, + const bool only_in_route = false); lanelet::ConstLanelets extendPrevLane( - const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes); + const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes, + const bool only_in_route = false); lanelet::ConstLanelets extendLanes( const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes); @@ -390,7 +391,7 @@ bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_thre double calcMinimumLaneChangeLength( const BehaviorPathPlannerParameters & common_param, const std::vector & shift_intervals, - const double length_to_intersection = 0.0); + const double backward_buffer, const double length_to_intersection = 0.0); lanelet::ConstLanelets getLaneletsFromPath( const PathWithLaneId & path, const std::shared_ptr & route_handler); diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 24ee1056e5a47..1f1eed3f95ec2 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -18,7 +18,10 @@ Takamasa Horibe Satoshi Ota - Yutaka Shimizu + Zulfaqar Azmi + Satoshi Ota + Mamoru Sobue + Takayuki Murooka @@ -46,6 +49,7 @@ autoware_auto_planning_msgs autoware_auto_tf2 autoware_auto_vehicle_msgs + autoware_perception_msgs behaviortree_cpp_v3 freespace_planning_algorithms geometry_msgs 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 0777822650dfb..ef44bee663985 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -106,6 +106,10 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod costmap_subscriber_ = create_subscription( "~/input/costmap", 1, std::bind(&BehaviorPathPlannerNode::onCostMap, this, _1), createSubscriptionOptions(this)); + traffic_signals_subscriber_ = + this->create_subscription( + "~/input/traffic_signals", 1, std::bind(&BehaviorPathPlannerNode::onTrafficSignals, this, _1), + createSubscriptionOptions(this)); lateral_offset_subscriber_ = this->create_subscription( "~/input/lateral_offset", 1, std::bind(&BehaviorPathPlannerNode::onLateralOffset, this, _1), createSubscriptionOptions(this)); @@ -277,6 +281,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.verbose = declare_parameter("verbose"); p.max_iteration_num = declare_parameter("max_iteration_num"); + p.traffic_light_signal_timeout = declare_parameter("traffic_light_signal_timeout"); const auto get_scene_module_manager_param = [&](std::string && ns) { ModuleConfigParameters config; @@ -340,6 +345,8 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() // lane change parameters p.backward_length_buffer_for_end_of_lane = declare_parameter("lane_change.backward_length_buffer_for_end_of_lane"); + p.backward_length_buffer_for_blocking_object = + declare_parameter("lane_change.backward_length_buffer_for_blocking_object"); p.lane_changing_lateral_jerk = declare_parameter("lane_change.lane_changing_lateral_jerk"); p.lane_change_prepare_duration = declare_parameter("lane_change.prepare_duration"); @@ -932,6 +939,17 @@ void BehaviorPathPlannerNode::onCostMap(const OccupancyGrid::ConstSharedPtr msg) const std::lock_guard lock(mutex_pd_); planner_data_->costmap = msg; } +void BehaviorPathPlannerNode::onTrafficSignals(const TrafficSignalArray::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_pd_); + + for (const auto & signal : msg->signals) { + TrafficSignalStamped traffic_signal; + traffic_signal.stamp = msg->stamp; + traffic_signal.signal = signal; + planner_data_->traffic_light_id_map[signal.traffic_signal_id] = traffic_signal; + } +} void BehaviorPathPlannerNode::onMap(const HADMapBin::ConstSharedPtr msg) { const std::lock_guard lock(mutex_map_); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 84064a1c38bcb..2ad0928696a5c 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -288,6 +288,12 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat data.reference_path, 0, data.reference_path.points.size(), calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0)); + data.to_return_point = utils::avoidance::calcDistanceToReturnDeadLine( + data.current_lanelets, data.reference_path_rough, planner_data_, parameters_); + + data.to_start_point = utils::avoidance::calcDistanceToAvoidStartLine( + data.current_lanelets, data.reference_path_rough, planner_data_, parameters_); + // target objects for avoidance fillAvoidanceTargetObjects(data, debug); @@ -631,7 +637,10 @@ void AvoidanceModule::fillDebugData( const auto o_front = data.stop_target_object.get(); const auto object_type = utils::getHighestProbLabel(o_front.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto & base_link2front = planner_data_->parameters.base_link2front; + const auto & additional_buffer_longitudinal = + object_parameter.use_conservative_buffer_longitudinal + ? planner_data_->parameters.base_link2front + : 0.0; const auto & vehicle_width = planner_data_->parameters.vehicle_width; const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o_front.distance_factor + @@ -640,7 +649,8 @@ void AvoidanceModule::fillDebugData( const auto variable = helper_.getSharpAvoidanceDistance( helper_.getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin)); const auto constant = helper_.getNominalPrepareDistance() + - object_parameter.safety_buffer_longitudinal + base_link2front; + object_parameter.safety_buffer_longitudinal + + additional_buffer_longitudinal; const auto total_avoid_distance = variable + constant; dead_pose_ = calcLongitudinalOffsetPose( @@ -894,7 +904,6 @@ AvoidOutlines AvoidanceModule::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_link2front = planner_data_->parameters.base_link2front; const auto & base_link2rear = planner_data_->parameters.base_link2rear; // Calculate feasible shift length @@ -932,8 +941,12 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( // calculate remaining distance. const auto prepare_distance = helper_.getNominalPrepareDistance(); - const auto constant = - object_parameter.safety_buffer_longitudinal + base_link2front + prepare_distance; + const auto & additional_buffer_longitudinal = + object_parameter.use_conservative_buffer_longitudinal + ? planner_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; @@ -1053,19 +1066,41 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( AvoidLine al_avoid; { - const auto offset = object_parameter.safety_buffer_longitudinal + base_link2front; + const auto & additional_buffer_longitudinal = + object_parameter.use_conservative_buffer_longitudinal + ? planner_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 path_front_to_ego = avoid_data_.arclength_from_ego.at(avoid_data_.ego_closest_path_index); - al_avoid.start_longitudinal = - std::max(o.longitudinal - offset - feasible_avoid_distance, 1e-3); + // start point (use previous linear shift length as start shift length.) + al_avoid.start_longitudinal = [&]() { + const auto nearest_avoid_distance = std::max(to_shift_end - feasible_avoid_distance, 1e-3); + + if (data.to_start_point > to_shift_end) { + return nearest_avoid_distance; + } + + const auto minimum_avoid_distance = + helper_.getMinAvoidanceDistance(feasible_shift_length.get() - current_ego_shift); + const auto furthest_avoid_distance = std::max(to_shift_end - minimum_avoid_distance, 1e-3); + + return std::clamp(data.to_start_point, nearest_avoid_distance, furthest_avoid_distance); + }(); + al_avoid.start_idx = utils::avoidance::findPathIndexFromArclength( avoid_data_.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego); al_avoid.start = avoid_data_.reference_path.points.at(al_avoid.start_idx).point.pose; al_avoid.start_shift_length = helper_.getLinearShift(al_avoid.start.position); + // end point al_avoid.end_shift_length = feasible_shift_length.get(); - al_avoid.end_longitudinal = o.longitudinal - offset; + al_avoid.end_longitudinal = to_shift_end; + + // misc al_avoid.id = getOriginalShiftLineUniqueId(); al_avoid.object = o; al_avoid.object_on_right = utils::avoidance::isOnRight(o); @@ -1074,18 +1109,24 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( AvoidLine al_return; { const auto offset = object_parameter.safety_buffer_longitudinal + base_link2rear + o.length; - // The end_margin also has the purpose of preventing the return path from NOT being - // triggered at the end point. - const auto return_remaining_distance = std::max( - data.arclength_from_ego.back() - o.longitudinal - offset - - parameters_->remain_buffer_distance, - 0.0); + const auto to_shift_start = o.longitudinal + offset; + // start point al_return.start_shift_length = feasible_shift_length.get(); + al_return.start_longitudinal = to_shift_start; + + // end point + al_return.end_longitudinal = [&]() { + if (data.to_return_point > to_shift_start) { + return std::clamp( + data.to_return_point, to_shift_start, feasible_return_distance + to_shift_start); + } + + return to_shift_start + feasible_return_distance; + }(); al_return.end_shift_length = 0.0; - al_return.start_longitudinal = o.longitudinal + offset; - al_return.end_longitudinal = - o.longitudinal + offset + std::min(feasible_return_distance, return_remaining_distance); + + // misc al_return.id = getOriginalShiftLineUniqueId(); al_return.object = o; al_return.object_on_right = utils::avoidance::isOnRight(o); @@ -1783,7 +1824,9 @@ AvoidLineArray AvoidanceModule::addReturnShiftLine( return ret; } - const auto remaining_distance = arclength_from_ego.back() - parameters_->remain_buffer_distance; + const auto remaining_distance = std::min( + arclength_from_ego.back() - parameters_->dead_line_buffer_for_goal, + avoid_data_.to_return_point); // If the avoidance point has already been set, the return shift must be set after the point. const auto last_sl_distance = avoid_data_.arclength_from_ego.at(last_sl.end_idx); @@ -1924,7 +1967,14 @@ bool AvoidanceModule::isSafePath( const auto is_object_front = utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info); + const auto & object_twist = object.initial_twist.twist; + const auto v_norm = std::hypot(object_twist.linear.x, object_twist.linear.y); + const auto object_type = utils::getHighestProbLabel(object.classification); + const auto object_parameter = parameters_->object_parameters.at(object_type); + const auto is_object_moving = v_norm > object_parameter.moving_speed_threshold; + const auto is_object_oncoming = + is_object_moving && utils::path_safety_checker::isTargetObjectOncoming(getEgoPose(), object.initial_pose.pose); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( @@ -2807,7 +2857,6 @@ void AvoidanceModule::updateAvoidanceDebugData( double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const { const auto & p = parameters_; - const auto & base_link2front = planner_data_->parameters.base_link2front; const auto & vehicle_width = planner_data_->parameters.vehicle_width; // D5 @@ -2823,7 +2872,8 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const // D2: min_avoid_distance // D3: longitudinal_avoid_margin_front (margin + D5) // D4: o_front.longitudinal - // D5: base_link2front + // D5: additional_buffer_longitudinal (base_link2front or 0 depending on the + // use_conservative_buffer_longitudinal) const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); @@ -2832,8 +2882,12 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; const auto variable = 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 + - base_link2front + p->stop_buffer; + additional_buffer_longitudinal + p->stop_buffer; return object.longitudinal - std::min(variable + constant, p->stop_max_distance); } @@ -2843,8 +2897,8 @@ void AvoidanceModule::insertReturnDeadLine( { const auto & data = avoid_data_; - if (!planner_data_->route_handler->isInGoalRouteSection(data.current_lanelets.back())) { - RCLCPP_DEBUG(getLogger(), "goal is far enough."); + if (data.to_return_point > planner_data_->parameters.forward_path_length) { + RCLCPP_DEBUG(getLogger(), "return dead line is far enough."); return; } @@ -2855,11 +2909,15 @@ void AvoidanceModule::insertReturnDeadLine( return; } - const auto min_return_distance = helper_.getMinAvoidanceDistance(shift_length); - - const auto to_goal = calcSignedArcLength( + // Consider the difference in path length between the shifted path and original path (the path + // that is shifted inward has a shorter distance to the end of the path than the other one.) + const auto & to_reference_path_end = data.arclength_from_ego.back(); + const auto to_shifted_path_end = calcSignedArcLength( shifted_path.path.points, getEgoPosition(), shifted_path.path.points.size() - 1); - const auto to_stop_line = to_goal - min_return_distance - parameters_->remain_buffer_distance; + const auto buffer = std::max(0.0, to_shifted_path_end - to_reference_path_end); + + const auto min_return_distance = helper_.getMinAvoidanceDistance(shift_length); + const auto to_stop_line = data.to_return_point - min_return_distance - buffer; // If we don't need to consider deceleration constraints, insert a deceleration point // and return immediately diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 763cf17890721..6ed67fc817f28 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -43,8 +43,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( getOrDeclareParameter(*node, ns + "resample_interval_for_output"); p.enable_bound_clipping = getOrDeclareParameter(*node, ns + "enable_bound_clipping"); p.enable_cancel_maneuver = getOrDeclareParameter(*node, ns + "enable_cancel_maneuver"); - p.enable_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "enable_force_avoidance_for_stopped_vehicle"); p.enable_yield_maneuver = getOrDeclareParameter(*node, ns + "enable_yield_maneuver"); p.enable_yield_maneuver_during_shifting = getOrDeclareParameter(*node, ns + "enable_yield_maneuver_during_shifting"); @@ -82,6 +80,8 @@ AvoidanceModuleManager::AvoidanceModuleManager( getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); param.safety_buffer_longitudinal = getOrDeclareParameter(*node, ns + "safety_buffer_longitudinal"); + param.use_conservative_buffer_longitudinal = + getOrDeclareParameter(*node, ns + "use_conservative_buffer_longitudinal"); return param; }; @@ -106,14 +106,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( // target filtering { std::string ns = "avoidance.target_filtering."; - p.threshold_time_force_avoidance_for_stopped_vehicle = getOrDeclareParameter( - *node, ns + "threshold_time_force_avoidance_for_stopped_vehicle"); - p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter( - *node, ns + "object_ignore_section_traffic_light_in_front_distance"); - p.object_ignore_section_crosswalk_in_front_distance = getOrDeclareParameter( - *node, ns + "object_ignore_section_crosswalk_in_front_distance"); - p.object_ignore_section_crosswalk_behind_distance = - getOrDeclareParameter(*node, ns + "object_ignore_section_crosswalk_behind_distance"); p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.threshold_distance_object_is_on_center = @@ -126,6 +118,20 @@ AvoidanceModuleManager::AvoidanceModuleManager( getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); } + { + 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.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 = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.front_distance"); + p.object_ignore_section_crosswalk_behind_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); + } + { std::string ns = "avoidance.target_filtering.detection_area."; p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); @@ -217,13 +223,23 @@ AvoidanceModuleManager::AvoidanceModuleManager( std::string ns = "avoidance.avoidance.longitudinal."; p.prepare_time = getOrDeclareParameter(*node, ns + "prepare_time"); p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); - p.remain_buffer_distance = getOrDeclareParameter(*node, ns + "remain_buffer_distance"); p.min_slow_down_speed = getOrDeclareParameter(*node, ns + "min_slow_down_speed"); p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); p.nominal_avoidance_speed = getOrDeclareParameter(*node, ns + "nominal_avoidance_speed"); } + // avoidance maneuver (return shift dead line) + { + std::string ns = "avoidance.avoidance.return_dead_line."; + p.enable_dead_line_for_goal = getOrDeclareParameter(*node, ns + "goal.enable"); + p.enable_dead_line_for_traffic_light = + getOrDeclareParameter(*node, ns + "traffic_light.enable"); + p.dead_line_buffer_for_goal = getOrDeclareParameter(*node, ns + "goal.buffer"); + p.dead_line_buffer_for_traffic_light = + getOrDeclareParameter(*node, ns + "traffic_light.buffer"); + } + // yield { std::string ns = "avoidance.yield."; @@ -336,6 +352,9 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "safety_buffer_lateral", config.safety_buffer_lateral); updateParam( parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal); + updateParam( + parameters, ns + "use_conservative_buffer_longitudinal", + config.use_conservative_buffer_longitudinal); }; { diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 64504918efd0e..6d34ce4ecde38 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -22,6 +22,7 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" #include @@ -113,6 +114,13 @@ GoalPlannerModule::GoalPlannerModule( freespace_parking_timer_cb_group_); } + // Initialize safety checker + if (parameters_->safety_check_params.enable_safety_check) { + initializeSafetyCheckParameters(); + utils::start_goal_planner_common::initializeCollisionCheckDebugMap( + goal_planner_data_.collision_check); + } + status_.reset(); } @@ -138,6 +146,17 @@ void GoalPlannerModule::onTimer() if (status_.get_goal_candidates().empty()) { return; } + + if (!isExecutionRequested()) { + return; + } + + if ( + !planner_data_ || + !goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + return; + } + const auto goal_candidates = status_.get_goal_candidates(); // generate valid pull over path candidates and calculate closest start pose @@ -219,16 +238,27 @@ void GoalPlannerModule::onFreespaceParkingTimer() } } -BehaviorModuleOutput GoalPlannerModule::run() +void GoalPlannerModule::updateData() { - current_state_ = ModuleStatus::RUNNING; - updateOccupancyGrid(); + if (getCurrentStatus() == ModuleStatus::IDLE) { + return; + } - if (!isActivated()) { - return planWaitingApproval(); + // Initialize Occupancy Grid Map + // This operation requires waiting for `planner_data_`, hence it is executed here instead of in + // the constructor. Ideally, this operation should only need to be performed once. + if ( + parameters_->use_occupancy_grid_for_goal_search || + parameters_->use_occupancy_grid_for_path_collision_check) { + initializeOccupancyGridMap(); } - return plan(); + updateOccupancyGrid(); + + // set current road lanes, pull over lanes, and drivable lane + setLanes(); + + generateGoalCandidates(); } void GoalPlannerModule::initializeOccupancyGridMap() @@ -255,27 +285,13 @@ void GoalPlannerModule::initializeSafetyCheckParameters() objects_filtering_params_, parameters_); } -void GoalPlannerModule::processOnEntry() -{ - // Initialize occupancy grid map - if ( - parameters_->use_occupancy_grid_for_goal_search || - parameters_->use_occupancy_grid_for_path_collision_check) { - initializeOccupancyGridMap(); - } - // Initialize safety checker - if (parameters_->safety_check_params.enable_safety_check) { - initializeSafetyCheckParameters(); - utils::start_goal_planner_common::initializeCollisionCheckDebugMap( - goal_planner_data_.collision_check); - } -} - void GoalPlannerModule::processOnExit() { resetPathCandidate(); resetPathReference(); debug_marker_.markers.clear(); + status_.reset(); + last_approval_data_.reset(); } bool GoalPlannerModule::isExecutionRequested() const @@ -284,6 +300,8 @@ bool GoalPlannerModule::isExecutionRequested() const return true; } + // TODO(someone): if goal is behind of ego, do not execute goal_planner + const auto & route_handler = planner_data_->route_handler; const Pose & current_pose = planner_data_->self_odometry->pose.pose; const Pose goal_pose = route_handler->getOriginalGoalPose(); @@ -437,13 +455,6 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const return refined_goal_pose; } -ModuleStatus GoalPlannerModule::updateState() -{ - // start_planner module will be run when setting new goal, so not need finishing pull_over module. - // Finishing it causes wrong lane_following path generation. - return current_state_; -} - bool GoalPlannerModule::planFreespacePath() { goal_searcher_->setPlannerData(planner_data_); @@ -546,6 +557,11 @@ void GoalPlannerModule::generateGoalCandidates() goal_searcher_->setPlannerData(planner_data_); goal_searcher_->setReferenceGoal(status_.get_refined_goal_pose()); status_.set_goal_candidates(goal_searcher_->search()); + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length, false); + status_.set_closest_goal_candidate_pose( + goal_searcher_->getClosetGoalCandidateAlongLanes(status_.get_goal_candidates()).goal_pose); } else { GoalCandidate goal_candidate{}; goal_candidate.goal_pose = goal_pose; @@ -553,6 +569,7 @@ void GoalPlannerModule::generateGoalCandidates() GoalCandidates goal_candidates{}; goal_candidates.push_back(goal_candidate); status_.set_goal_candidates(goal_candidates); + status_.set_closest_goal_candidate_pose(goal_pose); } } @@ -561,8 +578,6 @@ BehaviorModuleOutput GoalPlannerModule::plan() resetPathCandidate(); resetPathReference(); - generateGoalCandidates(); - path_reference_ = getPreviousModuleOutput().reference_path; if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { @@ -760,6 +775,7 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) } else { // not_safe -> not_safe: use previous stop path output.path = status_.get_prev_stop_path(); + stop_pose_ = utils::getFirstStopPoseFromPath(*output.path); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); } @@ -789,6 +805,7 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output } else { // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path output.path = status_.get_prev_stop_path_after_approval(); + stop_pose_ = utils::getFirstStopPoseFromPath(*output.path); RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path"); } output.reference_path = getPreviousModuleOutput().reference_path; @@ -897,29 +914,32 @@ void GoalPlannerModule::decideVelocity() status_.set_has_decided_velocity(true); } +CandidateOutput GoalPlannerModule::planCandidate() const +{ + return CandidateOutput( + status_.get_pull_over_path() ? status_.get_pull_over_path()->getFullPath() : PathWithLaneId()); +} + BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() { + // if pull over path candidates generation is not finished, use previous module output + if (status_.get_pull_over_path_candidates().empty()) { + return getPreviousModuleOutput(); + } + constexpr double path_update_duration = 1.0; resetPathCandidate(); resetPathReference(); - // set current road lanes, pull over lanes, and drivable lane - setLanes(); - // Check if it needs to decide path status_.set_has_decided_path(hasDecidedPath()); // Use decided path if (status_.get_has_decided_path()) { - if (isActivated() && isWaitingApproval()) { - { - const std::lock_guard lock(mutex_); - status_.set_last_approved_time(std::make_shared(clock_->now())); - status_.set_last_approved_pose( - std::make_shared(planner_data_->self_odometry->pose.pose)); - } - clearWaitingApproval(); + if (isActivated() && !last_approval_data_) { + last_approval_data_ = + std::make_unique(clock_->now(), planner_data_->self_odometry->pose.pose); decideVelocity(); } transitionToNextPathIfFinishingCurrentPath(); @@ -935,7 +955,7 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() // set output and status BehaviorModuleOutput output{}; setOutput(output); - path_candidate_ = std::make_shared(status_.get_pull_over_path()->getFullPath()); + path_candidate_ = std::make_shared(planCandidate().path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; // return to lane parking if it is possible @@ -981,17 +1001,16 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification() { - waitApproval(); + // if pull over path candidates generation is not finished, use previous module output + if (status_.get_pull_over_path_candidates().empty()) { + return getPreviousModuleOutput(); + } - updateOccupancyGrid(); BehaviorModuleOutput out; - out.modified_goal = plan().modified_goal; // update status_ + out.modified_goal = planWithGoalModification().modified_goal; // update status_ out.path = std::make_shared(generateStopPath()); out.reference_path = getPreviousModuleOutput().reference_path; - path_candidate_ = - status_.get_is_safe_static_objects() - ? std::make_shared(status_.get_pull_over_path()->getFullPath()) - : out.path; + path_candidate_ = std::make_shared(planCandidate().path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; const auto distance_to_path_change = calcDistanceToPathChange(); @@ -1024,7 +1043,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( std::pair GoalPlannerModule::calcDistanceToPathChange() const { - const auto & full_path = status_.get_pull_over_path()->getFullPath(); + const auto full_path = status_.get_pull_over_path()->getFullPath(); const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( full_path.points, planner_data_->self_odometry->pose.pose, std::numeric_limits::max(), @@ -1081,20 +1100,23 @@ PathWithLaneId GoalPlannerModule::generateStopPath() // difference between the outer and inner sides) // 4. feasible stop const auto search_start_offset_pose = calcLongitudinalOffsetPose( - reference_path.points, status_.get_refined_goal_pose().position, - -parameters_->backward_goal_search_length - common_parameters.base_link2front - - approximate_pull_over_distance_); + reference_path.points, status_.get_closest_goal_candidate_pose().position, + -approximate_pull_over_distance_); if ( !status_.get_is_safe_static_objects() && !status_.get_closest_start_pose() && !search_start_offset_pose) { return generateFeasibleStopPath(); } - const Pose stop_pose = - status_.get_is_safe_static_objects() - ? status_.get_pull_over_path()->start_pose - : (status_.get_closest_start_pose() ? status_.get_closest_start_pose().value() - : *search_start_offset_pose); + const Pose stop_pose = [&]() -> Pose { + if (status_.get_is_safe_static_objects()) { + return status_.get_pull_over_path()->start_pose; + } + if (status_.get_closest_start_pose()) { + return status_.get_closest_start_pose().value(); + } + return *search_start_offset_pose; + }(); // if stop pose is closer than min_stop_distance, stop as soon as possible const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, stop_pose); @@ -1165,12 +1187,11 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath() { - if (isActivated() && status_.get_last_approved_time()) { + if (isActivated() && last_approval_data_) { // if using arc_path and finishing current_path, get next path // enough time for turn signal - const bool has_passed_enough_time = - (clock_->now() - *status_.get_last_approved_time()).seconds() > - planner_data_->parameters.turn_signal_search_time; + const bool has_passed_enough_time = (clock_->now() - last_approval_data_->time).seconds() > + planner_data_->parameters.turn_signal_search_time; if (hasFinishedCurrentPath() && has_passed_enough_time && status_.get_require_increment()) { if (incrementPathIndex()) { @@ -1305,10 +1326,9 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const { // ego decelerates so that current pose is the point `turn_light_on_threshold_time` seconds // before starting pull_over - turn_signal.desired_start_point = - status_.get_last_approved_pose() && status_.get_has_decided_path() - ? *status_.get_last_approved_pose() - : current_pose; + turn_signal.desired_start_point = last_approval_data_ && status_.get_has_decided_path() + ? last_approval_data_->pose + : current_pose; turn_signal.desired_end_point = end_pose; turn_signal.required_start_point = start_pose; turn_signal.required_end_point = end_pose; @@ -1325,7 +1345,6 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const return true; } } - if (!parameters_->use_object_recognition) { return false; } @@ -1339,20 +1358,41 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const utils::path_safety_checker::isPolygonOverlapLanelet); const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_over_lane_objects, parameters_->th_moving_object_velocity); - const auto common_parameters = planner_data_->parameters; - const double base_link2front = common_parameters.base_link2front; - const double base_link2rear = common_parameters.base_link2rear; - const double vehicle_width = common_parameters.vehicle_width; - - const auto ego_polygons_expanded = - utils::path_safety_checker::generatePolygonsWithStoppingAndInertialMargin( - path, base_link2front, base_link2rear, vehicle_width, parameters_->maximum_deceleration, + std::vector obj_polygons; + for (const auto & object : pull_over_lane_stop_objects.objects) { + obj_polygons.push_back(tier4_autoware_utils::toPolygon2d(object)); + } + + std::vector ego_polygons_expanded; + const auto curvatures = motion_utils::calcCurvature(path.points); + for (size_t i = 0; i < path.points.size(); ++i) { + const auto p = path.points.at(i); + + const double extra_stopping_margin = std::min( + std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_max_extra_stopping_margin); + + double extra_lateral_margin = (-1) * curvatures.at(i) * p.point.longitudinal_velocity_mps * + std::abs(p.point.longitudinal_velocity_mps); + extra_lateral_margin = + std::clamp(extra_lateral_margin, -extra_stopping_margin, extra_stopping_margin); + + const auto lateral_offset_pose = + tier4_autoware_utils::calcOffsetPose(p.point.pose, 0.0, extra_lateral_margin / 2.0, 0.0); + const auto ego_polygon = tier4_autoware_utils::toFootprint( + lateral_offset_pose, + planner_data_->parameters.base_link2front + + parameters_->object_recognition_collision_check_margin + extra_stopping_margin, + planner_data_->parameters.base_link2rear + + parameters_->object_recognition_collision_check_margin, + planner_data_->parameters.vehicle_width + + parameters_->object_recognition_collision_check_margin * 2.0 + + std::abs(extra_lateral_margin)); + ego_polygons_expanded.push_back(ego_polygon); + } debug_data_.ego_polygons_expanded = ego_polygons_expanded; - return utils::path_safety_checker::checkCollisionWithMargin( - ego_polygons_expanded, pull_over_lane_stop_objects, - parameters_->object_recognition_collision_check_margin); + return utils::path_safety_checker::checkPolygonsIntersects(ego_polygons_expanded, obj_polygons); } bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) const @@ -1365,7 +1405,7 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c // so need enough distance to restart. // distance to restart should be less than decide_path_distance. // otherwise, the goal would change immediately after departure. - const bool is_separated_path = status_.get_pull_over_path()->partial_paths.size() > 1; + const bool is_separated_path = pull_over_path.partial_paths.size() > 1; const double distance_to_start = calcSignedArcLength( pull_over_path.getFullPath().points, current_pose.position, pull_over_path.start_pose.position); const double distance_to_restart = parameters_->decide_path_distance / 2; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 22c8ac022ad1a..05b7c5d7dec92 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -76,25 +76,35 @@ bool LaneChangeInterface::isExecutionReady() const ModuleStatus LaneChangeInterface::updateState() { + auto log_warn_throttled = [&](const std::string & message) -> void { + RCLCPP_WARN_STREAM_THROTTLE( + getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, message); + }; + if (module_type_->specialExpiredCheck()) { + log_warn_throttled("expired check."); if (isWaitingApproval()) { return ModuleStatus::SUCCESS; } } if (!isActivated() || isWaitingApproval()) { + log_warn_throttled("Is idling."); return ModuleStatus::IDLE; } if (!module_type_->isValidPath()) { + log_warn_throttled("Is invalid path."); return ModuleStatus::SUCCESS; } if (module_type_->isAbortState()) { + log_warn_throttled("Ego is in the process of aborting lane change."); return module_type_->hasFinishedAbort() ? ModuleStatus::SUCCESS : ModuleStatus::RUNNING; } if (module_type_->hasFinishedLaneChange()) { + log_warn_throttled("Completed lane change."); return ModuleStatus::SUCCESS; } @@ -102,31 +112,29 @@ ModuleStatus LaneChangeInterface::updateState() setObjectDebugVisualization(); if (is_safe) { + log_warn_throttled("Lane change path is safe."); module_type_->toNormalState(); return ModuleStatus::RUNNING; } - if (!module_type_->isCancelEnabled()) { - RCLCPP_WARN_STREAM_THROTTLE( - getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, - "Lane change path is unsafe but cancel was not enabled. Continue lane change."); + const auto change_state_if_stop_required = [&]() -> void { if (module_type_->isRequiredStop(is_object_coming_from_rear)) { module_type_->toStopState(); } else { module_type_->toNormalState(); } + }; + + if (!module_type_->isCancelEnabled()) { + log_warn_throttled( + "Lane change path is unsafe but cancel was not enabled. Continue lane change."); + change_state_if_stop_required(); return ModuleStatus::RUNNING; } if (!module_type_->isAbleToReturnCurrentLane()) { - RCLCPP_WARN_STREAM_THROTTLE( - getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, - "Lane change path is unsafe but cannot return. Continue lane change."); - if (module_type_->isRequiredStop(is_object_coming_from_rear)) { - module_type_->toStopState(); - } else { - module_type_->toNormalState(); - } + log_warn_throttled("Lane change path is unsafe but cannot return. Continue lane change."); + change_state_if_stop_required(); return ModuleStatus::RUNNING; } @@ -134,53 +142,33 @@ ModuleStatus LaneChangeInterface::updateState() const auto threshold = common_parameters.backward_length_buffer_for_end_of_lane; const auto status = module_type_->getLaneChangeStatus(); if (module_type_->isNearEndOfCurrentLanes(status.current_lanes, status.target_lanes, threshold)) { - RCLCPP_WARN_STREAM_THROTTLE( - getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, - "Lane change path is unsafe but near end of lane. Continue lane change."); - if (module_type_->isRequiredStop(is_object_coming_from_rear)) { - module_type_->toStopState(); - } else { - module_type_->toNormalState(); - } + log_warn_throttled("Lane change path is unsafe but near end of lane. Continue lane change."); + change_state_if_stop_required(); return ModuleStatus::RUNNING; } if (module_type_->isEgoOnPreparePhase() && module_type_->isAbleToReturnCurrentLane()) { - RCLCPP_WARN_STREAM_THROTTLE( - getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, - "Lane change path is unsafe. Cancel lane change."); + log_warn_throttled("Lane change path is unsafe. Cancel lane change."); module_type_->toCancelState(); return isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::SUCCESS; } if (!module_type_->isAbortEnabled()) { - RCLCPP_WARN_STREAM_THROTTLE( - getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, + log_warn_throttled( "Lane change path is unsafe but abort was not enabled. Continue lane change."); - if (module_type_->isRequiredStop(is_object_coming_from_rear)) { - module_type_->toStopState(); - } else { - module_type_->toNormalState(); - } + change_state_if_stop_required(); return ModuleStatus::RUNNING; } - const auto found_abort_path = module_type_->getAbortPath(); + const auto found_abort_path = module_type_->calcAbortPath(); if (!found_abort_path) { - RCLCPP_WARN_STREAM_THROTTLE( - getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, + log_warn_throttled( "Lane change path is unsafe but not found abort path. Continue lane change."); - if (module_type_->isRequiredStop(is_object_coming_from_rear)) { - module_type_->toStopState(); - } else { - module_type_->toNormalState(); - } + change_state_if_stop_required(); return ModuleStatus::RUNNING; } - RCLCPP_WARN_STREAM_THROTTLE( - getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, - "Lane change path is unsafe. Abort lane change."); + log_warn_throttled("Lane change path is unsafe. Abort lane change."); module_type_->toAbortState(); return ModuleStatus::RUNNING; } @@ -207,7 +195,6 @@ BehaviorModuleOutput LaneChangeInterface::plan() auto output = module_type_->generateOutput(); path_reference_ = output.reference_path; *prev_approved_path_ = *getPreviousModuleOutput().path; - module_type_->insertStopPoint(module_type_->getLaneChangeStatus().target_lanes, *output.path); stop_pose_ = module_type_->getStopPose(); @@ -453,8 +440,8 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( const auto & common_parameter = module_type_->getCommonParam(); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const double next_lane_change_buffer = - utils::calcMinimumLaneChangeLength(common_parameter, shift_intervals); + const double next_lane_change_buffer = utils::calcMinimumLaneChangeLength( + common_parameter, shift_intervals, common_parameter.backward_length_buffer_for_end_of_lane); const double & nearest_dist_threshold = common_parameter.ego_nearest_dist_threshold; const double & nearest_yaw_threshold = common_parameter.ego_nearest_yaw_threshold; const double & base_to_front = common_parameter.base_link2front; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 78e05c940a814..ee7b45de09e93 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -275,8 +275,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); p.resample_interval_for_output = getOrDeclareParameter(*node, ns + "resample_interval_for_output"); - p.enable_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "enable_force_avoidance_for_stopped_vehicle"); } // target object @@ -320,14 +318,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( // target filtering { std::string ns = "avoidance.target_filtering."; - p.threshold_time_force_avoidance_for_stopped_vehicle = getOrDeclareParameter( - *node, ns + "threshold_time_force_avoidance_for_stopped_vehicle"); - p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter( - *node, ns + "object_ignore_section_traffic_light_in_front_distance"); - p.object_ignore_section_crosswalk_in_front_distance = getOrDeclareParameter( - *node, ns + "object_ignore_section_crosswalk_in_front_distance"); - p.object_ignore_section_crosswalk_behind_distance = - getOrDeclareParameter(*node, ns + "object_ignore_section_crosswalk_behind_distance"); p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.threshold_distance_object_is_on_center = @@ -340,6 +330,20 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); } + { + 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.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 = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.front_distance"); + p.object_ignore_section_crosswalk_behind_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); + } + { std::string ns = "avoidance.target_filtering.detection_area."; p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index de120592e7505..5bffef1a4b421 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -127,38 +127,43 @@ bool NormalLaneChange::isLaneChangeRequired() const LaneChangePath NormalLaneChange::getLaneChangePath() const { - return isAbortState() ? *abort_path_ : status_.lane_change_path; + return status_.lane_change_path; } BehaviorModuleOutput NormalLaneChange::generateOutput() { BehaviorModuleOutput output; - output.path = std::make_shared(getLaneChangePath().path); - const auto found_extended_path = extendPath(); - if (found_extended_path) { - *output.path = utils::combinePath(*output.path, *found_extended_path); - } - extendOutputDrivableArea(output); - output.reference_path = std::make_shared(getReferencePath()); - output.turn_signal_info = updateOutputTurnSignal(); - - if (isAbortState()) { + if (isAbortState() && abort_path_) { + output.path = std::make_shared(abort_path_->path); output.reference_path = std::make_shared(prev_module_reference_path_); output.turn_signal_info = prev_turn_signal_info_; - return output; - } + insertStopPoint(status_.current_lanes, *output.path); + } else { + output.path = std::make_shared(getLaneChangePath().path); - if (isStopState()) { - const auto current_velocity = getEgoVelocity(); - const auto current_dist = calcSignedArcLength( - output.path->points, output.path->points.front().point.pose.position, getEgoPosition()); - const auto stop_dist = - -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); - const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, *output.path); - setStopPose(stop_point.point.pose); + const auto found_extended_path = extendPath(); + if (found_extended_path) { + *output.path = utils::combinePath(*output.path, *found_extended_path); + } + output.reference_path = std::make_shared(getReferencePath()); + output.turn_signal_info = updateOutputTurnSignal(); + + if (isStopState()) { + const auto current_velocity = getEgoVelocity(); + const auto current_dist = calcSignedArcLength( + output.path->points, output.path->points.front().point.pose.position, getEgoPosition()); + const auto stop_dist = + -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); + const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, *output.path); + setStopPose(stop_point.point.pose); + } else { + insertStopPoint(status_.target_lanes, *output.path); + } } + extendOutputDrivableArea(output); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points); output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( *output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info, @@ -200,8 +205,9 @@ void NormalLaneChange::insertStopPoint( } const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back()); - const double lane_change_buffer = - utils::calcMinimumLaneChangeLength(getCommonParam(), shift_intervals, 0.0); + const double lane_change_buffer = utils::calcMinimumLaneChangeLength( + getCommonParam(), shift_intervals, getCommonParam().backward_length_buffer_for_end_of_lane, + 0.0); const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); @@ -221,6 +227,23 @@ void NormalLaneChange::insertStopPoint( const auto target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes); double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + const auto is_valid_start_point = std::invoke([&]() -> bool { + auto lc_start_point = lanelet::utils::conversion::toLaneletPoint( + status_.lane_change_path.info.lane_changing_start.position); + const auto target_neighbor_preferred_lane_poly_2d = + utils::lane_change::getTargetNeighborLanesPolygon( + *route_handler, status_.current_lanes, type_); + return boost::geometry::covered_by( + lanelet::traits::to2D(lc_start_point), target_neighbor_preferred_lane_poly_2d); + }); + + if (!is_valid_start_point) { + const auto stop_point = utils::insertStopPoint(stopping_distance, path); + setStopPose(stop_point.point.pose); + + return; + } + // calculate minimum distance from path front to the stationary object on the ego lane. const auto distance_to_ego_lane_obj = [&]() -> double { double distance_to_obj = distance_to_terminal; @@ -263,10 +286,14 @@ void NormalLaneChange::insertStopPoint( const auto rss_dist = calcRssDistance( 0.0, planner_data_->parameters.minimum_lane_changing_velocity, lane_change_parameters_->rss_params); + const double lane_change_buffer_for_blocking_object = utils::calcMinimumLaneChangeLength( + getCommonParam(), shift_intervals, + getCommonParam().backward_length_buffer_for_blocking_object, 0.0); - const auto stopping_distance_for_obj = distance_to_ego_lane_obj - lane_change_buffer - - stop_point_buffer - rss_dist - - getCommonParam().base_link2front; + const auto stopping_distance_for_obj = + distance_to_ego_lane_obj - lane_change_buffer_for_blocking_object - + getCommonParam().backward_length_buffer_for_blocking_object - rss_dist - + getCommonParam().base_link2front; // If the target lane in the lane change section is blocked by a stationary obstacle, there // is no reason for stopping with a lane change margin. Instead, stop right behind the @@ -461,8 +488,9 @@ bool NormalLaneChange::isNearEndOfCurrentLanes( const auto & current_pose = getEgoPose(); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const auto lane_change_buffer = - utils::calcMinimumLaneChangeLength(planner_data_->parameters, shift_intervals); + const auto lane_change_buffer = utils::calcMinimumLaneChangeLength( + planner_data_->parameters, shift_intervals, + getCommonParam().backward_length_buffer_for_end_of_lane); auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); @@ -640,7 +668,8 @@ double NormalLaneChange::calcMaximumLaneChangeLength( const auto shift_intervals = getRouteHandler()->getLateralIntervalsToPreferredLane(current_terminal_lanelet); return utils::lane_change::calcMaximumLaneChangeLength( - getEgoVelocity(), getCommonParam(), shift_intervals, max_acc); + std::max(getCommonParam().minimum_lane_changing_velocity, getEgoVelocity()), getCommonParam(), + shift_intervals, max_acc); } std::vector NormalLaneChange::sampleLongitudinalAccValues( @@ -918,6 +947,24 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( return filtered_obj_indices; } +std::vector NormalLaneChange::filterObjectsInTargetLane( + const LaneChangeTargetObjects & objects, const lanelet::ConstLanelets & target_lanes) const +{ + const auto target_polygon = + utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); + std::vector filtered_objects{}; + if (target_polygon) { + for (auto & obj : objects.target_lane) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj.initial_pose.pose, obj.shape); + if (boost::geometry::intersects(target_polygon.value(), obj_polygon)) { + filtered_objects.push_back(obj); + } + } + } + + return filtered_objects; +} + PathWithLaneId NormalLaneChange::getTargetSegment( const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, const double lane_changing_length, @@ -971,8 +1018,8 @@ bool NormalLaneChange::hasEnoughLength( const double lane_change_length = path.info.length.sum(); const auto shift_intervals = route_handler.getLateralIntervalsToPreferredLane(target_lanes.back(), direction); - double minimum_lane_change_length_to_preferred_lane = - utils::calcMinimumLaneChangeLength(common_parameters, shift_intervals); + double minimum_lane_change_length_to_preferred_lane = utils::calcMinimumLaneChangeLength( + common_parameters, shift_intervals, common_parameters.backward_length_buffer_for_end_of_lane); if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { return false; @@ -1068,9 +1115,11 @@ bool NormalLaneChange::getLaneChangePaths( const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); const double lane_change_buffer = utils::calcMinimumLaneChangeLength( - common_parameters, route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); + common_parameters, route_handler.getLateralIntervalsToPreferredLane(current_lanes.back()), + common_parameters.backward_length_buffer_for_end_of_lane); const double next_lane_change_buffer = utils::calcMinimumLaneChangeLength( - common_parameters, route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); + common_parameters, route_handler.getLateralIntervalsToPreferredLane(target_lanes.back()), + common_parameters.backward_length_buffer_for_end_of_lane); const auto dist_to_end_of_current_lanes = utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); @@ -1224,6 +1273,20 @@ bool NormalLaneChange::getLaneChangePaths( boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); + LaneChangeInfo lane_change_info; + lane_change_info.longitudinal_acceleration = + LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing}; + lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time}; + lane_change_info.velocity = + LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity}; + lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; + lane_change_info.current_lanes = current_lanes; + lane_change_info.target_lanes = target_lanes; + lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose; + lane_change_info.lane_changing_end = target_segment.points.front().point.pose; + lane_change_info.lateral_acceleration = lateral_acc; + lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; + if (!is_valid_start_point) { debug_print( "Reject: lane changing points are not inside of the target preferred lanes or its " @@ -1243,21 +1306,8 @@ bool NormalLaneChange::getLaneChangePaths( continue; } - LaneChangeInfo lane_change_info; - lane_change_info.longitudinal_acceleration = - LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing}; - lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time}; - lane_change_info.velocity = - LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity}; - lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; - lane_change_info.current_lanes = current_lanes; - lane_change_info.target_lanes = target_lanes; - lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose; - lane_change_info.lane_changing_end = target_segment.points.front().point.pose; lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( prepare_segment, target_segment, target_lane_reference_path, shift_length); - lane_change_info.lateral_acceleration = lateral_acc; - lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; const auto candidate_path = utils::lane_change::constructCandidatePath( lane_change_info, prepare_segment, target_segment, target_lane_reference_path, @@ -1296,11 +1346,13 @@ bool NormalLaneChange::getLaneChangePaths( } candidate_paths->push_back(*candidate_path); + + std::vector filtered_objects = + filterObjectsInTargetLane(target_objects, target_lanes); if ( - !is_stuck && - utils::lane_change::passParkedObject( - route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer, - is_goal_in_route, *lane_change_parameters_, object_debug_)) { + !is_stuck && utils::lane_change::passParkedObject( + route_handler, *candidate_path, filtered_objects, lane_change_buffer, + is_goal_in_route, *lane_change_parameters_, object_debug_)) { debug_print( "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " "lane change."); @@ -1453,7 +1505,7 @@ bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) con isAbleToStopSafely() && is_object_coming_from_rear; } -bool NormalLaneChange::getAbortPath() +bool NormalLaneChange::calcAbortPath() { const auto & route_handler = getRouteHandler(); const auto & common_param = getCommonParam(); @@ -1469,8 +1521,8 @@ bool NormalLaneChange::getAbortPath() const auto direction = getDirection(); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane( selected_path.info.current_lanes.back(), direction); - const double minimum_lane_change_length = - utils::calcMinimumLaneChangeLength(common_param, shift_intervals); + const double minimum_lane_change_length = utils::calcMinimumLaneChangeLength( + common_param, shift_intervals, common_param.backward_length_buffer_for_end_of_lane); const auto & lane_changing_path = selected_path.path; const auto lane_changing_end_pose_idx = std::invoke([&]() { @@ -1561,14 +1613,12 @@ bool NormalLaneChange::getAbortPath() reference_lanelets, shifted_path.path.points.at(abort_return_idx).point.pose); const PathWithLaneId reference_lane_segment = std::invoke([&]() { const double s_start = arc_position.length; - double s_end = std::max( - lanelet::utils::getLaneletLength2d(reference_lanelets) - minimum_lane_change_length, s_start); + double s_end = std::max(lanelet::utils::getLaneletLength2d(reference_lanelets), s_start); if (route_handler->isInGoalRouteSection(selected_path.info.target_lanes.back())) { const auto goal_arc_coordinates = lanelet::utils::getArcCoordinates(reference_lanelets, route_handler->getGoalPose()); - const double forward_length = - std::max(goal_arc_coordinates.length - minimum_lane_change_length, s_start); + const double forward_length = std::max(goal_arc_coordinates.length, s_start); s_end = std::min(s_end, forward_length); } PathWithLaneId ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end, true); @@ -1578,20 +1628,23 @@ bool NormalLaneChange::getAbortPath() return ref; }); - PathWithLaneId start_to_abort_return_pose; - start_to_abort_return_pose.points.insert( - start_to_abort_return_pose.points.end(), shifted_path.path.points.begin(), - shifted_path.path.points.begin() + abort_return_idx); - if (reference_lane_segment.points.size() > 1) { - start_to_abort_return_pose.points.insert( - start_to_abort_return_pose.points.end(), (reference_lane_segment.points.begin() + 1), - reference_lane_segment.points.end()); - } - auto abort_path = selected_path; abort_path.shifted_path = shifted_path; - abort_path.path = start_to_abort_return_pose; abort_path.info.shift_line = shift_line; + + { + PathWithLaneId aborting_path; + aborting_path.points.insert( + aborting_path.points.begin(), shifted_path.path.points.begin(), + shifted_path.path.points.begin() + abort_return_idx); + + if (!reference_lane_segment.points.empty()) { + abort_path.path = utils::combinePath(aborting_path, reference_lane_segment); + } else { + abort_path.path = aborting_path; + } + } + abort_path_ = std::make_shared(abort_path); return true; } @@ -1723,8 +1776,9 @@ bool NormalLaneChange::isVehicleStuck( : utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const double lane_change_buffer = - utils::calcMinimumLaneChangeLength(getCommonParam(), shift_intervals, 0.0); + const double lane_change_buffer = utils::calcMinimumLaneChangeLength( + getCommonParam(), shift_intervals, getCommonParam().backward_length_buffer_for_end_of_lane, + 0.0); const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane; const double terminal_judge_buffer = lane_change_buffer + stop_point_buffer + 1.0; if (distance_to_terminal < terminal_judge_buffer) { diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index 30810bda252c3..46db10417d19e 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -320,7 +320,7 @@ bool StartPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const const auto start_planner_ptr = std::dynamic_pointer_cast(observer.lock()); // Currently simultaneous execution with other modules is not supported while backward driving - if (!start_planner_ptr->isBackFinished()) { + if (!start_planner_ptr->isDrivingForward()) { return false; } @@ -349,7 +349,7 @@ bool StartPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() cons const auto start_planner_ptr = std::dynamic_pointer_cast(observer.lock()); // Currently simultaneous execution with other modules is not supported while backward driving - if (!start_planner_ptr->isBackFinished()) { + if (start_planner_ptr->isDrivingForward()) { return false; } diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 62f3dbfe191ac..e3670710b64f0 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -135,8 +135,8 @@ void StartPlannerModule::updateData() if (has_received_new_route) { status_ = PullOutStatus(); } - // check safety status after back finished - if (parameters_->safety_check_params.enable_safety_check && status_.back_finished) { + // check safety status when driving forward + if (parameters_->safety_check_params.enable_safety_check && status_.driving_forward) { status_.is_safe_dynamic_objects = isSafePath(); } else { status_.is_safe_dynamic_objects = true; @@ -145,54 +145,79 @@ void StartPlannerModule::updateData() bool StartPlannerModule::isExecutionRequested() const { - const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); - const double lateral_distance_to_center_lane = - lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance; - - if (std::abs(lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road) { - return false; + if (isModuleRunning()) { + return true; } - const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); + // Return false and do not request execution if any of the following conditions are true: + // - The start pose is on the middle of the road. + // - The vehicle has already arrived at the start position planner. + // - The vehicle has reached the goal position. + // - The vehicle is still moving. if ( - tier4_autoware_utils::calcDistance2d(start_pose.position, current_pose.position) > - parameters_->th_arrived_distance) { + isCurrentPoseOnMiddleOfTheRoad() || isCloseToOriginalStartPose() || hasArrivedAtGoal() || + isMoving()) { return false; } - // Check if ego arrives at goal - const Pose goal_pose = planner_data_->route_handler->getGoalPose(); - if ( - tier4_autoware_utils::calcDistance2d(goal_pose.position, current_pose.position) < - parameters_->th_arrived_distance) { + // Check if the goal is behind the ego vehicle within the same route segment. + if (IsGoalBehindOfEgoInSameRouteSegment()) { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); return false; } - if (current_state_ == ModuleStatus::RUNNING) { - return true; - } + return true; +} - const bool is_stopped = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear) < - parameters_->th_stopped_velocity; - if (!is_stopped) { - return false; - } +bool StartPlannerModule::isModuleRunning() const +{ + return current_state_ == ModuleStatus::RUNNING; +} - return true; +bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const +{ + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + const double lateral_distance_to_center_lane = + lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance; + + return std::abs(lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road; +} + +bool StartPlannerModule::isCloseToOriginalStartPose() const +{ + const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); + return tier4_autoware_utils::calcDistance2d( + start_pose.position, planner_data_->self_odometry->pose.pose.position) > + parameters_->th_arrived_distance; +} + +bool StartPlannerModule::hasArrivedAtGoal() const +{ + const Pose goal_pose = planner_data_->route_handler->getGoalPose(); + return tier4_autoware_utils::calcDistance2d( + goal_pose.position, planner_data_->self_odometry->pose.pose.position) < + parameters_->th_arrived_distance; +} + +bool StartPlannerModule::isMoving() const +{ + return utils::l2Norm(planner_data_->self_odometry->twist.twist.linear) >= + parameters_->th_stopped_velocity; } bool StartPlannerModule::isExecutionReady() const { - // when is_safe_static_objects is false,the path is not generated and approval shouldn't be + // when found_pull_out_path is false,the path is not generated and approval shouldn't be // allowed - if (!status_.is_safe_static_objects) { - RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against static objects"); + if (!status_.found_pull_out_path) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Pull over path is not found"); return false; } if ( - parameters_->safety_check_params.enable_safety_check && status_.back_finished && + parameters_->safety_check_params.enable_safety_check && status_.driving_forward && isWaitingApproval()) { if (!isSafePath()) { RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); @@ -234,15 +259,6 @@ void StartPlannerModule::updateCurrentState() BehaviorModuleOutput StartPlannerModule::plan() { - if (IsGoalBehindOfEgoInSameRouteSegment()) { - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); - const auto output = generateStopOutput(); - setDebugData(); // use status updated in generateStopOutput() - updateRTCStatus(0, 0); - return output; - } - if (isWaitingApproval()) { clearWaitingApproval(); resetPathCandidate(); @@ -250,7 +266,7 @@ BehaviorModuleOutput StartPlannerModule::plan() } BehaviorModuleOutput output; - if (!status_.is_safe_static_objects) { + if (!status_.found_pull_out_path) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); const auto output = generateStopOutput(); @@ -262,7 +278,7 @@ BehaviorModuleOutput StartPlannerModule::plan() PathWithLaneId path; // Check if backward motion is finished - if (status_.back_finished) { + if (status_.driving_forward) { // Increment path index if the current path is finished if (hasFinishedCurrentPath()) { RCLCPP_INFO(getLogger(), "Increment path index"); @@ -317,7 +333,7 @@ BehaviorModuleOutput StartPlannerModule::plan() return SteeringFactor::STRAIGHT; }); - if (status_.back_finished) { + if (status_.driving_forward) { const double start_distance = motion_utils::calcSignedArcLength( path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); @@ -369,7 +385,7 @@ PathWithLaneId StartPlannerModule::getFullPath() const pull_out_path.points.end(), partial_path.points.begin(), partial_path.points.end()); } - if (status_.back_finished) { + if (status_.driving_forward) { // not need backward path or finish it return pull_out_path; } @@ -385,18 +401,8 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() { updatePullOutStatus(); - if (IsGoalBehindOfEgoInSameRouteSegment()) { - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); - clearWaitingApproval(); - const auto output = generateStopOutput(); - setDebugData(); // use status updated in generateStopOutput() - updateRTCStatus(0, 0); - return output; - } - BehaviorModuleOutput output; - if (!status_.is_safe_static_objects) { + if (!status_.found_pull_out_path) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); clearWaitingApproval(); @@ -414,7 +420,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() planner_data_, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); - auto stop_path = status_.back_finished ? getCurrentPath() : status_.backward_path; + auto stop_path = status_.driving_forward ? getCurrentPath() : status_.backward_path; const auto drivable_lanes = generateDrivableLanes(stop_path); const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto expanded_lanes = utils::expandLanelets( @@ -441,7 +447,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() return SteeringFactor::STRAIGHT; }); - if (status_.back_finished) { + if (status_.driving_forward) { const double start_distance = motion_utils::calcSignedArcLength( stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); @@ -492,102 +498,112 @@ void StartPlannerModule::planWithPriority( const std::vector & start_pose_candidates, const Pose & refined_start_pose, const Pose & goal_pose, const std::string search_priority) { - // check if start pose candidates are valid - if (start_pose_candidates.empty()) { - return; - } - - const auto is_safe_with_pose_planner = [&](const size_t i, const auto & planner) { - // Get the pull_out_start_pose for the current index - const auto & pull_out_start_pose = start_pose_candidates.at(i); - - // Set back_finished to true if the current start pose is same to refined_start_pose - status_.back_finished = - tier4_autoware_utils::calcDistance2d(pull_out_start_pose, refined_start_pose) < 0.01; - - planner->setPlannerData(planner_data_); - const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose); - // not found safe path - if (!pull_out_path) { - return false; - } - // use current path if back is not needed - if (status_.back_finished) { - const std::lock_guard lock(mutex_); - status_.is_safe_static_objects = true; - status_.pull_out_path = *pull_out_path; - status_.pull_out_start_pose = pull_out_start_pose; - status_.planner_type = planner->getPlannerType(); - return true; - } + if (start_pose_candidates.empty()) return; - // If this is the last start pose candidate, return false - if (i == start_pose_candidates.size() - 1) return false; + const PriorityOrder order_priority = + determinePriorityOrder(search_priority, start_pose_candidates.size()); - // check next path if back is needed - const auto & pull_out_start_pose_next = start_pose_candidates.at(i + 1); - const auto pull_out_path_next = planner->plan(pull_out_start_pose_next, goal_pose); - // not found safe path - if (!pull_out_path_next) { - return false; - } + for (const auto & [index, planner] : order_priority) { + if (findPullOutPath(start_pose_candidates, index, planner, refined_start_pose, goal_pose)) + return; + } - // Update status variables with the next path information - { - const std::lock_guard lock(mutex_); - status_.is_safe_static_objects = true; - status_.pull_out_path = *pull_out_path_next; - status_.pull_out_start_pose = pull_out_start_pose_next; - status_.planner_type = planner->getPlannerType(); - } - return true; - }; + updateStatusIfNoSafePathFound(); +} - using PriorityOrder = std::vector>>; - const auto make_loop_order_planner_first = [&]() { - PriorityOrder order_priority; +PriorityOrder StartPlannerModule::determinePriorityOrder( + const std::string & search_priority, const size_t candidates_size) +{ + PriorityOrder order_priority; + if (search_priority == "efficient_path") { for (const auto & planner : start_planners_) { - for (size_t i = 0; i < start_pose_candidates.size(); i++) { + for (size_t i = 0; i < candidates_size; i++) { order_priority.emplace_back(i, planner); } } - return order_priority; - }; - - const auto make_loop_order_pose_first = [&]() { - PriorityOrder order_priority; - for (size_t i = 0; i < start_pose_candidates.size(); i++) { + } else if (search_priority == "short_back_distance") { + for (size_t i = 0; i < candidates_size; i++) { for (const auto & planner : start_planners_) { order_priority.emplace_back(i, planner); } } - return order_priority; - }; - - // Choose loop order based on priority_on_efficient_path - PriorityOrder order_priority; - if (search_priority == "efficient_path") { - order_priority = make_loop_order_planner_first(); - } else if (search_priority == "short_back_distance") { - order_priority = make_loop_order_pose_first(); } else { - RCLCPP_ERROR( - getLogger(), - "search_priority should be efficient_path or short_back_distance, but %s is given.", - search_priority.c_str()); + RCLCPP_ERROR(getLogger(), "Invalid search_priority: %s", search_priority.c_str()); throw std::domain_error("[start_planner] invalid search_priority"); } + return order_priority; +} - for (const auto & p : order_priority) { - if (is_safe_with_pose_planner(p.first, p.second)) { - return; - } +bool StartPlannerModule::findPullOutPath( + const std::vector & start_pose_candidates, const size_t index, + const std::shared_ptr & planner, const Pose & refined_start_pose, + const Pose & goal_pose) +{ + // Ensure the index is within the bounds of the start_pose_candidates vector + if (index >= start_pose_candidates.size()) return false; + + const Pose & pull_out_start_pose = start_pose_candidates.at(index); + const bool is_driving_forward = + tier4_autoware_utils::calcDistance2d(pull_out_start_pose, refined_start_pose) < 0.01; + + planner->setPlannerData(planner_data_); + const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose); + + // If no path is found, return false + if (!pull_out_path) { + return false; } - // not found safe path + // If driving forward, update status with the current path and return true + if (is_driving_forward) { + updateStatusWithCurrentPath(*pull_out_path, pull_out_start_pose, planner->getPlannerType()); + return true; + } + + // If this is the last start pose candidate, return false + if (index == start_pose_candidates.size() - 1) return false; + + const Pose & next_pull_out_start_pose = start_pose_candidates.at(index + 1); + const auto next_pull_out_path = planner->plan(next_pull_out_start_pose, goal_pose); + + // If no next path is found, return false + if (!next_pull_out_path) return false; + + // Update status with the next path and return true + updateStatusWithNextPath( + *next_pull_out_path, next_pull_out_start_pose, planner->getPlannerType()); + return true; +} + +void StartPlannerModule::updateStatusWithCurrentPath( + const behavior_path_planner::PullOutPath & path, const Pose & start_pose, + const behavior_path_planner::PlannerType & planner_type) +{ + const std::lock_guard lock(mutex_); + status_.driving_forward = true; + status_.found_pull_out_path = true; + status_.pull_out_path = path; + status_.pull_out_start_pose = start_pose; + status_.planner_type = planner_type; +} + +void StartPlannerModule::updateStatusWithNextPath( + const behavior_path_planner::PullOutPath & path, const Pose & start_pose, + const behavior_path_planner::PlannerType & planner_type) +{ + const std::lock_guard lock(mutex_); + status_.driving_forward = false; + status_.found_pull_out_path = true; + status_.pull_out_path = path; + status_.pull_out_start_pose = start_pose; + status_.planner_type = planner_type; +} + +void StartPlannerModule::updateStatusIfNoSafePathFound() +{ if (status_.planner_type != PlannerType::FREESPACE) { const std::lock_guard lock(mutex_); - status_.is_safe_static_objects = false; + status_.found_pull_out_path = false; status_.planner_type = PlannerType::NONE; } } @@ -730,7 +746,7 @@ void StartPlannerModule::updatePullOutStatus() void StartPlannerModule::updateStatusAfterBackwardDriving() { - status_.back_finished = true; + status_.driving_forward = true; status_.backward_driving_complete = true; // request start_planner approval waitApproval(); @@ -835,7 +851,7 @@ bool StartPlannerModule::isOverlappedWithLane( bool StartPlannerModule::hasFinishedPullOut() const { - if (!status_.back_finished || !status_.is_safe_static_objects) { + if (!status_.driving_forward || !status_.found_pull_out_path) { return false; } @@ -874,7 +890,7 @@ bool StartPlannerModule::isBackwardDrivingComplete() const const double ego_vel = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear); const bool is_stopped = ego_vel < parameters_->th_stopped_velocity; - const bool back_finished = !status_.back_finished && is_near && is_stopped; + const bool back_finished = !status_.driving_forward && is_near && is_stopped; if (back_finished) { RCLCPP_INFO(getLogger(), "back finished"); } @@ -916,7 +932,7 @@ bool StartPlannerModule::isStuck() } // not found safe path - if (!status_.is_safe_static_objects) { + if (!status_.found_pull_out_path) { return true; } @@ -943,7 +959,7 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const const Pose & end_pose = status_.pull_out_path.end_pose; // turn on hazard light when backward driving - if (!status_.back_finished) { + 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; @@ -1157,7 +1173,7 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput() { const std::lock_guard lock(mutex_); - status_.back_finished = true; + status_.driving_forward = true; status_.planner_type = PlannerType::STOP; status_.pull_out_path.partial_paths.clear(); status_.pull_out_path.partial_paths.push_back(stop_path); @@ -1209,8 +1225,8 @@ bool StartPlannerModule::planFreespacePath() status_.pull_out_path = *freespace_path; status_.pull_out_start_pose = current_pose; status_.planner_type = freespace_planner_->getPlannerType(); - status_.is_safe_static_objects = true; - status_.back_finished = true; + status_.found_pull_out_path = true; + status_.driving_forward = true; return true; } @@ -1231,7 +1247,7 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; output.drivable_area_info = - status_.back_finished + status_.driving_forward ? utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info) : current_drivable_area_info; @@ -1291,13 +1307,13 @@ void StartPlannerModule::setDebugData() const const auto header = planner_data_->route_handler->getRouteHeader(); { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = status_.is_safe_static_objects ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = status_.found_pull_out_path ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); marker.pose = status_.pull_out_start_pose; - if (!status_.back_finished) { + if (!status_.driving_forward) { marker.text = "BACK -> "; } marker.text += magic_enum::enum_name(status_.planner_type); diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index 3fe30b11e3a57..d1b8bdc806bf4 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -165,30 +165,16 @@ boost::optional TurnSignalDecider::getIntersectionTurnSignalInfo const std::string lane_attribute = current_lane.attributeOr("turn_direction", std::string("none")); - // check next lanes - auto next_lanes = route_handler.getNextLanelets(current_lane); - if (next_lanes.empty()) { + // check next lane has the same attribute + lanelet::ConstLanelet next_lane{}; + if (!route_handler.getNextLaneletWithinRoute(current_lane, &next_lane)) { break; } - - // filter next lanes with same attribute in the path - std::vector next_lanes_in_path{}; - std::copy_if( - next_lanes.begin(), next_lanes.end(), std::back_inserter(next_lanes_in_path), - [&](const auto & next_lane) { - const bool is_in_unique_ids = - std::find(unique_lane_ids.begin(), unique_lane_ids.end(), next_lane.id()) != - unique_lane_ids.end(); - const bool has_same_attribute = - next_lane.attributeOr("turn_direction", std::string("none")) == lane_attribute; - return is_in_unique_ids && has_same_attribute; - }); - if (next_lanes_in_path.empty()) { + if (next_lane.attributeOr("turn_direction", std::string("none")) != lane_attribute) { break; } - // assume that the next lane in the path is only one - current_lane = next_lanes_in_path.front(); + current_lane = next_lane; } if (!combined_lane_elems.empty()) { diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index a9f40dad86ae4..ce4c9ad7ac72d 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/utils/avoidance/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/traffic_light_utils.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -48,6 +49,9 @@ namespace behavior_path_planner::utils::avoidance { +using autoware_perception_msgs::msg::TrafficSignalElement; +using behavior_path_planner::utils::traffic_light::calcDistanceToRedTrafficLight; +using behavior_path_planner::utils::traffic_light::getDistanceToNextTrafficLight; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; @@ -225,6 +229,7 @@ void pushUniqueVector(T & base_vector, const T & additional_vector) { base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end()); } + } // namespace bool isOnRight(const ObjectData & obj) @@ -292,6 +297,81 @@ bool isWithinCrosswalk( return false; } +bool isWithinIntersection( + const ObjectData & object, const std::shared_ptr & route_handler) +{ + const std::string id = object.overhang_lanelet.attributeOr("intersection_area", "else"); + if (id == "else") { + return false; + } + + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object); + + const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); + + return boost::geometry::within( + object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon()))); +} + +bool isForceAvoidanceTarget( + ObjectData & object, const lanelet::ConstLanelets & extend_lanelets, + 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) { + return false; + } + + if (object.is_within_intersection) { + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is in the intersection area."); + 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; + } + + 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; + + // check traffic light + const auto to_traffic_light = getDistanceToNextTrafficLight(object_pose, extend_lanelets); + { + not_parked_object = + to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance; + } + + // check crosswalk + const auto to_crosswalk = + utils::getDistanceToCrosswalk(ego_pose, extend_lanelets, *rh->getOverallGraphPtr()) - + object.longitudinal; + { + 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; + } + + object.to_stop_factor_distance = std::min(to_traffic_light, to_crosswalk); + + return !not_parked_object; +} + double calcShiftLength( const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin) { @@ -679,26 +759,41 @@ void fillObjectEnvelopePolygon( return; } - const auto envelope_poly = + const auto one_shot_envelope_poly = createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin); - if (boost::geometry::within(envelope_poly, same_id_obj->envelope_poly)) { + // If the one_shot_envelope_poly is within the registered envelope, use the registered one + if (boost::geometry::within(one_shot_envelope_poly, same_id_obj->envelope_poly)) { object_data.envelope_poly = same_id_obj->envelope_poly; return; } std::vector unions; - boost::geometry::union_(envelope_poly, same_id_obj->envelope_poly, unions); + boost::geometry::union_(one_shot_envelope_poly, same_id_obj->envelope_poly, unions); + // If union fails, use the current envelope if (unions.empty()) { - object_data.envelope_poly = - createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin); + object_data.envelope_poly = one_shot_envelope_poly; return; } boost::geometry::correct(unions.front()); - object_data.envelope_poly = createEnvelopePolygon(unions.front(), closest_pose, 0.0); + const auto multi_step_envelope_poly = createEnvelopePolygon(unions.front(), closest_pose, 0.0); + + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object); + const auto object_polygon_area = boost::geometry::area(object_polygon); + const auto envelope_polygon_area = boost::geometry::area(multi_step_envelope_poly); + + // keep multi-step envelope polygon. + constexpr double THRESHOLD = 5.0; + if (envelope_polygon_area < object_polygon_area * THRESHOLD) { + object_data.envelope_poly = multi_step_envelope_poly; + return; + } + + // use latest one-shot envelope polygon. + object_data.envelope_poly = one_shot_envelope_poly; } void fillObjectMovingTime( @@ -1037,25 +1132,38 @@ void filterTargetObjects( }; // current lanelet - update_road_to_shoulder_distance(overhang_lanelet); + { + update_road_to_shoulder_distance(overhang_lanelet); + + o.to_road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( + rh, target_line, o.to_road_shoulder_distance, overhang_lanelet, o.overhang_pose.position, + overhang_basic_pose, parameters->use_hatched_road_markings, + parameters->use_intersection_areas); + } + // previous lanelet lanelet::ConstLanelets previous_lanelet{}; if (rh->getPreviousLaneletsWithinRoute(overhang_lanelet, &previous_lanelet)) { update_road_to_shoulder_distance(previous_lanelet.front()); + + o.to_road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( + rh, target_line, o.to_road_shoulder_distance, previous_lanelet.front(), + o.overhang_pose.position, overhang_basic_pose, parameters->use_hatched_road_markings, + parameters->use_intersection_areas); } + // next lanelet lanelet::ConstLanelet next_lanelet{}; if (rh->getNextLaneletWithinRoute(overhang_lanelet, &next_lanelet)) { update_road_to_shoulder_distance(next_lanelet); - } - debug.bounds.push_back(target_line); - { o.to_road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( - rh, target_line, o.to_road_shoulder_distance, overhang_lanelet, o.overhang_pose.position, + rh, target_line, o.to_road_shoulder_distance, next_lanelet, o.overhang_pose.position, overhang_basic_pose, parameters->use_hatched_road_markings, parameters->use_intersection_areas); } + + debug.bounds.push_back(target_line); } // calculate avoid_margin dynamically @@ -1114,42 +1222,14 @@ void filterTargetObjects( continue; } - // from here condition check for vehicle type objects. - - const auto stop_time_longer_than_threshold = - o.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle; - - if (stop_time_longer_than_threshold && parameters->enable_force_avoidance_for_stopped_vehicle) { - // force avoidance for stopped vehicle - bool not_parked_object = true; + o.is_within_intersection = isWithinIntersection(o, rh); - // check traffic light - const auto to_traffic_light = - utils::getDistanceToNextTrafficLight(object_pose, extend_lanelets); - { - not_parked_object = - to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance; - } - - // check crosswalk - const auto to_crosswalk = - utils::getDistanceToCrosswalk(ego_pose, extend_lanelets, *rh->getOverallGraphPtr()) - - o.longitudinal; - { - 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; - } - - o.to_stop_factor_distance = std::min(to_traffic_light, to_crosswalk); - - if (!not_parked_object) { - o.last_seen = now; - o.avoid_margin = avoid_margin; - data.target_objects.push_back(o); - continue; - } + // from here condition check for vehicle type objects. + if (isForceAvoidanceTarget(o, extend_lanelets, planner_data, parameters)) { + o.last_seen = now; + o.avoid_margin = avoid_margin; + data.target_objects.push_back(o); + continue; } // Object is on center line -> ignore. @@ -1827,4 +1907,63 @@ DrivableLanes generateExpandDrivableLanes( return current_drivable_lanes; } + +double calcDistanceToAvoidStartLine( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + if (lanelets.empty()) { + return std::numeric_limits::lowest(); + } + + double distance_to_return_dead_line = std::numeric_limits::lowest(); + + // dead line stop factor(traffic light) + if (parameters->enable_dead_line_for_traffic_light) { + const auto to_traffic_light = calcDistanceToRedTrafficLight(lanelets, path, planner_data); + if (to_traffic_light.has_value()) { + distance_to_return_dead_line = std::max( + distance_to_return_dead_line, + to_traffic_light.value() + parameters->dead_line_buffer_for_traffic_light); + } + } + + return distance_to_return_dead_line; +} + +double calcDistanceToReturnDeadLine( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + if (lanelets.empty()) { + return std::numeric_limits::max(); + } + + double distance_to_return_dead_line = std::numeric_limits::max(); + + // dead line stop factor(traffic light) + if (parameters->enable_dead_line_for_traffic_light) { + const auto to_traffic_light = calcDistanceToRedTrafficLight(lanelets, path, planner_data); + if (to_traffic_light.has_value()) { + distance_to_return_dead_line = std::min( + distance_to_return_dead_line, + to_traffic_light.value() - parameters->dead_line_buffer_for_traffic_light); + } + } + + // dead line for goal + if (parameters->enable_dead_line_for_goal) { + if (planner_data->route_handler->isInGoalRouteSection(lanelets.back())) { + const auto & ego_pos = planner_data->self_odometry->pose.pose.position; + const auto to_goal_distance = + calcSignedArcLength(path.points, ego_pos, path.points.size() - 1); + distance_to_return_dead_line = std::min( + distance_to_return_dead_line, to_goal_distance - parameters->dead_line_buffer_for_goal); + } + } + + return distance_to_return_dead_line; +} } // namespace behavior_path_planner::utils::avoidance diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 1e6dc1776359b..f498ecd9ed8ec 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -468,4 +468,30 @@ bool GoalSearcher::isInAreas(const LinearRing2d & footprint, const BasicPolygons return false; } +GoalCandidate GoalSearcher::getClosetGoalCandidateAlongLanes( + const GoalCandidates & goal_candidates) const +{ + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, + /*forward_only_in_route*/ false); + + // Define a lambda function to compute the arc length for a given goal candidate. + auto getGoalArcLength = [¤t_lanes](const auto & candidate) { + return lanelet::utils::getArcCoordinates(current_lanes, candidate.goal_pose).length; + }; + + // Find the closest goal candidate by comparing the arc lengths of each candidate. + const auto closest_goal_candidate = std::min_element( + goal_candidates.begin(), goal_candidates.end(), + [&getGoalArcLength](const auto & a, const auto & b) { + return getGoalArcLength(a) < getGoalArcLength(b); + }); + + if (closest_goal_candidate == goal_candidates.end()) { + return {}; // return empty GoalCandidate in case no valid candidates are found. + } + + return *closest_goal_candidate; +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index d5867c40824d2..fb2a96f1d5cbd 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -604,8 +604,8 @@ bool hasEnoughLengthToLaneChangeAfterAbort( { const auto shift_intervals = route_handler.getLateralIntervalsToPreferredLane(current_lanes.back(), direction); - const double minimum_lane_change_length = - utils::calcMinimumLaneChangeLength(common_param, shift_intervals); + const double minimum_lane_change_length = utils::calcMinimumLaneChangeLength( + common_param, shift_intervals, common_param.backward_length_buffer_for_end_of_lane); const auto abort_plus_lane_change_length = abort_return_dist + minimum_lane_change_length; if (abort_plus_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { return false; diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index a7c604985de3a..f6644a4f91e8f 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -285,6 +285,7 @@ ExtendedPredictedObject transform( extended_object.initial_twist = object.kinematics.initial_twist_with_covariance; extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance; extended_object.shape = object.shape; + extended_object.classification = object.classification; const auto obj_velocity = extended_object.initial_twist.twist.linear.x; diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 8ebc144a34584..40aa184105edb 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -18,7 +18,7 @@ #include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include +#include #include #include @@ -380,44 +380,12 @@ std::vector getCollidedPolygons( return collided_polygons; } -std::vector generatePolygonsWithStoppingAndInertialMargin( - const PathWithLaneId & ego_path, const double base_to_front, const double base_to_rear, - const double width, const double maximum_deceleration, const double max_extra_stopping_margin) +bool checkPolygonsIntersects( + const std::vector & polys_1, const std::vector & polys_2) { - std::vector polygons; - const auto curvatures = motion_utils::calcCurvature(ego_path.points); - - for (size_t i = 0; i < ego_path.points.size(); ++i) { - const auto p = ego_path.points.at(i); - - const double extra_stopping_margin = std::min( - std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / maximum_deceleration, - max_extra_stopping_margin); - - double extra_lateral_margin = (-1) * curvatures[i] * p.point.longitudinal_velocity_mps * - std::abs(p.point.longitudinal_velocity_mps); - extra_lateral_margin = - std::clamp(extra_lateral_margin, -extra_stopping_margin, extra_stopping_margin); - - const auto lateral_offset_pose = - tier4_autoware_utils::calcOffsetPose(p.point.pose, 0.0, extra_lateral_margin / 2.0, 0.0); - const auto ego_polygon = tier4_autoware_utils::toFootprint( - lateral_offset_pose, base_to_front + extra_stopping_margin, base_to_rear, - width + std::abs(extra_lateral_margin)); - polygons.push_back(ego_polygon); - } - return polygons; -} - -bool checkCollisionWithMargin( - const std::vector & ego_polygons, const PredictedObjects & dynamic_objects, - const double collision_check_margin) -{ - for (const auto & ego_polygon : ego_polygons) { - for (const auto & object : dynamic_objects.objects) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); - const double distance = boost::geometry::distance(obj_polygon, ego_polygon); - if (distance < collision_check_margin) { + for (const auto & poly_1 : polys_1) { + for (const auto & poly_2 : polys_2) { + if (boost::geometry::intersects(poly_1, poly_2)) { return true; } } diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index 3ac3c09ba0d97..a6d7bc0040c23 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -580,4 +580,13 @@ PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & return filtered_path; } +boost::optional getFirstStopPoseFromPath(const PathWithLaneId & path) +{ + for (const auto & p : path.points) { + if (std::abs(p.point.longitudinal_velocity_mps) < 0.01) { + return p.point.pose; + } + } + return boost::none; +} } // namespace behavior_path_planner::utils diff --git a/planning/behavior_path_planner/src/utils/traffic_light_utils.cpp b/planning/behavior_path_planner/src/utils/traffic_light_utils.cpp new file mode 100644 index 0000000000000..9ec8531818d83 --- /dev/null +++ b/planning/behavior_path_planner/src/utils/traffic_light_utils.cpp @@ -0,0 +1,157 @@ +// Copyright 2023 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 +#include + +namespace behavior_path_planner::utils::traffic_light +{ +using motion_utils::calcSignedArcLength; + +bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color) +{ + const auto it_lamp = + std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { + return x.shape == TrafficSignalElement::CIRCLE && x.color == lamp_color; + }); + + return it_lamp != tl_state.elements.end(); +} + +bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape) +{ + const auto it_lamp = std::find_if( + tl_state.elements.begin(), tl_state.elements.end(), + [&lamp_shape](const auto & x) { return x.shape == lamp_shape; }); + + return it_lamp != tl_state.elements.end(); +} + +bool isTrafficSignalStop(const lanelet::ConstLanelet & lanelet, const TrafficSignal & tl_state) +{ + if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::GREEN)) { + return false; + } + + if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::UNKNOWN)) { + return false; + } + + const std::string turn_direction = lanelet.attributeOr("turn_direction", "else"); + + if (turn_direction == "else") { + return true; + } + if ( + turn_direction == "right" && + hasTrafficLightShape(tl_state, TrafficSignalElement::RIGHT_ARROW)) { + return false; + } + if ( + turn_direction == "left" && hasTrafficLightShape(tl_state, TrafficSignalElement::LEFT_ARROW)) { + return false; + } + if ( + turn_direction == "straight" && + hasTrafficLightShape(tl_state, TrafficSignalElement::UP_ARROW)) { + return false; + } + + return true; +} + +double getDistanceToNextTrafficLight( + const Pose & current_pose, const lanelet::ConstLanelets & lanelets) +{ + lanelet::ConstLanelet current_lanelet; + if (!lanelet::utils::query::getClosestLanelet(lanelets, current_pose, ¤t_lanelet)) { + return std::numeric_limits::infinity(); + } + + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(current_pose.position); + const auto to_object = lanelet::geometry::toArcCoordinates( + lanelet::utils::to2D(current_lanelet.centerline()), + lanelet::utils::to2D(lanelet_point).basicPoint()); + + for (const auto & element : current_lanelet.regulatoryElementsAs()) { + lanelet::ConstLineString3d lanelet_stop_lines = element->stopLine().get(); + + const auto to_stop_line = lanelet::geometry::toArcCoordinates( + lanelet::utils::to2D(current_lanelet.centerline()), + lanelet::utils::to2D(lanelet_stop_lines).front().basicPoint()); + + const auto distance_object_to_stop_line = to_stop_line.length - to_object.length; + + if (distance_object_to_stop_line > 0.0) { + return distance_object_to_stop_line; + } + } + + double distance = lanelet::utils::getLaneletLength3d(current_lanelet); + + bool found_current_lane = false; + for (const auto & llt : lanelets) { + if (llt.id() == current_lanelet.id()) { + found_current_lane = true; + continue; + } + + if (!found_current_lane) { + continue; + } + + for (const auto & element : llt.regulatoryElementsAs()) { + lanelet::ConstLineString3d lanelet_stop_lines = element->stopLine().get(); + + const auto to_stop_line = lanelet::geometry::toArcCoordinates( + lanelet::utils::to2D(llt.centerline()), + lanelet::utils::to2D(lanelet_stop_lines).front().basicPoint()); + + return distance + to_stop_line.length - to_object.length; + } + + distance += lanelet::utils::getLaneletLength3d(llt); + } + + return std::numeric_limits::infinity(); +} + +std::optional calcDistanceToRedTrafficLight( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data) +{ + for (const auto & lanelet : lanelets) { + for (const auto & element : lanelet.regulatoryElementsAs()) { + const auto traffic_signal_stamped = planner_data->getTrafficSignal(element->id()); + if (!traffic_signal_stamped.has_value()) { + continue; + } + + if (!isTrafficSignalStop(lanelet, traffic_signal_stamped.value().signal)) { + continue; + } + + const auto & ego_pos = planner_data->self_odometry->pose.pose.position; + lanelet::ConstLineString3d stop_line = *(element->stopLine()); + const auto x = 0.5 * (stop_line.front().x() + stop_line.back().x()); + const auto y = 0.5 * (stop_line.front().y() + stop_line.back().y()); + const auto z = 0.5 * (stop_line.front().z() + stop_line.back().z()); + + return calcSignedArcLength(path.points, ego_pos, tier4_autoware_utils::createPoint(x, y, z)); + } + } + + return std::nullopt; +} +} // namespace behavior_path_planner::utils::traffic_light diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 9b10dfc32457f..62a48da85ba2c 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -974,6 +974,9 @@ BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr(reference_path); output.reference_path = std::make_shared(reference_path); output.drivable_area_info.drivable_lanes = drivable_lanes; @@ -2289,62 +2292,6 @@ double getDistanceToEndOfLane(const Pose & current_pose, const lanelet::ConstLan return lanelet_length - arc_coordinates.length; } -double getDistanceToNextTrafficLight( - const Pose & current_pose, const lanelet::ConstLanelets & lanelets) -{ - lanelet::ConstLanelet current_lanelet; - if (!lanelet::utils::query::getClosestLanelet(lanelets, current_pose, ¤t_lanelet)) { - return std::numeric_limits::infinity(); - } - - const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(current_pose.position); - const auto to_object = lanelet::geometry::toArcCoordinates( - lanelet::utils::to2D(current_lanelet.centerline()), - lanelet::utils::to2D(lanelet_point).basicPoint()); - - for (const auto & element : current_lanelet.regulatoryElementsAs()) { - lanelet::ConstLineString3d lanelet_stop_lines = element->stopLine().get(); - - const auto to_stop_line = lanelet::geometry::toArcCoordinates( - lanelet::utils::to2D(current_lanelet.centerline()), - lanelet::utils::to2D(lanelet_stop_lines).front().basicPoint()); - - const auto distance_object_to_stop_line = to_stop_line.length - to_object.length; - - if (distance_object_to_stop_line > 0.0) { - return distance_object_to_stop_line; - } - } - - double distance = lanelet::utils::getLaneletLength3d(current_lanelet); - - bool found_current_lane = false; - for (const auto & llt : lanelets) { - if (llt.id() == current_lanelet.id()) { - found_current_lane = true; - continue; - } - - if (!found_current_lane) { - continue; - } - - for (const auto & element : llt.regulatoryElementsAs()) { - lanelet::ConstLineString3d lanelet_stop_lines = element->stopLine().get(); - - const auto to_stop_line = lanelet::geometry::toArcCoordinates( - lanelet::utils::to2D(llt.centerline()), - lanelet::utils::to2D(lanelet_stop_lines).front().basicPoint()); - - return distance + to_stop_line.length - to_object.length; - } - - distance += lanelet::utils::getLaneletLength3d(llt); - } - - return std::numeric_limits::infinity(); -} - double getDistanceToNextIntersection( const Pose & current_pose, const lanelet::ConstLanelets & lanelets) { @@ -2708,6 +2655,17 @@ Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet) : tier4_autoware_utils::inverseClockwise(polygon); } +Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon) +{ + Polygon2d ret; + for (const auto & p : polygon) { + ret.outer().emplace_back(p.x(), p.y()); + } + ret.outer().push_back(ret.outer().front()); + + return tier4_autoware_utils::isClockwise(ret) ? ret : tier4_autoware_utils::inverseClockwise(ret); +} + std::vector getTargetLaneletPolygons( const lanelet::PolygonLayer & map_polygons, lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length, const std::string & target_type) @@ -3081,48 +3039,85 @@ lanelet::ConstLanelets getCurrentLanesFromPath( lanelet::ConstLanelet current_lane; lanelet::utils::query::getClosestLanelet(reference_lanes, current_pose, ¤t_lane); - - return route_handler->getLaneletSequence( + auto current_lanes = route_handler->getLaneletSequence( current_lane, current_pose, p.backward_path_length, p.forward_path_length); + + // Extend the 'current_lanes' with previous lanes until it contains 'front_lane_ids' + // if the extended prior lanes is in same lane sequence with current lanes + const auto front_lane_ids = path.points.front().lane_ids; + auto have_front_lanes = [front_lane_ids](const auto & lanes) { + return std::any_of(lanes.begin(), lanes.end(), [&](const auto & lane) { + return std::find(front_lane_ids.begin(), front_lane_ids.end(), lane.id()) != + front_lane_ids.end(); + }); + }; + auto extended_lanes = current_lanes; + while (rclcpp::ok()) { + const size_t pre_extension_size = extended_lanes.size(); // Get existing size before extension + extended_lanes = extendPrevLane(route_handler, extended_lanes, true); + if (extended_lanes.size() == pre_extension_size) break; + if (have_front_lanes(extended_lanes)) { + current_lanes = extended_lanes; + break; + } + } + + return current_lanes; } lanelet::ConstLanelets extendNextLane( - const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes) + const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes, + const bool only_in_route) { + if (lanes.empty()) return lanes; + auto extended_lanes = lanes; // Add next lane const auto next_lanes = route_handler->getNextLanelets(extended_lanes.back()); if (!next_lanes.empty()) { + boost::optional target_next_lane; + if (!only_in_route) { + target_next_lane = next_lanes.front(); + } // use the next lane in route if it exists - auto target_next_lane = next_lanes.front(); for (const auto & next_lane : next_lanes) { if (route_handler->isRouteLanelet(next_lane)) { target_next_lane = next_lane; } } - extended_lanes.push_back(target_next_lane); + if (target_next_lane) { + extended_lanes.push_back(*target_next_lane); + } } return extended_lanes; } lanelet::ConstLanelets extendPrevLane( - const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes) + const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes, + const bool only_in_route) { + if (lanes.empty()) return lanes; + auto extended_lanes = lanes; // Add previous lane const auto prev_lanes = route_handler->getPreviousLanelets(extended_lanes.front()); if (!prev_lanes.empty()) { + boost::optional target_prev_lane; + if (!only_in_route) { + target_prev_lane = prev_lanes.front(); + } // use the previous lane in route if it exists - auto target_prev_lane = prev_lanes.front(); for (const auto & prev_lane : prev_lanes) { if (route_handler->isRouteLanelet(prev_lane)) { target_prev_lane = prev_lane; } } - extended_lanes.insert(extended_lanes.begin(), target_prev_lane); + if (target_prev_lane) { + extended_lanes.insert(extended_lanes.begin(), *target_prev_lane); + } } return extended_lanes; } @@ -3273,7 +3268,7 @@ bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_thre double calcMinimumLaneChangeLength( const BehaviorPathPlannerParameters & common_param, const std::vector & shift_intervals, - const double length_to_intersection) + const double backward_buffer, const double length_to_intersection) { if (shift_intervals.empty()) { return 0.0; @@ -3291,8 +3286,7 @@ double calcMinimumLaneChangeLength( PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, max_lateral_acc); accumulated_length += vel * t + finish_judge_buffer; } - accumulated_length += - common_param.backward_length_buffer_for_end_of_lane * (shift_intervals.size() - 1.0); + accumulated_length += backward_buffer * (shift_intervals.size() - 1.0); return accumulated_length; } diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_intersection_module/CMakeLists.txt index 8ff78dac06716..9e7eb196cd0c1 100644 --- a/planning/behavior_velocity_intersection_module/CMakeLists.txt +++ b/planning/behavior_velocity_intersection_module/CMakeLists.txt @@ -20,3 +20,8 @@ target_link_libraries(${PROJECT_NAME} ) ament_auto_package(INSTALL_TO_SHARE config) + +install(PROGRAMS + scripts/ttc.py + DESTINATION lib/${PROJECT_NAME} +) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index fde55dc7a6c55..adbc7b3e087d6 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -56,6 +56,8 @@ object_dist_to_stopline: 10.0 # [m] ignore_on_amber_traffic_light: object_expected_deceleration: 2.0 # [m/ss] + ignore_on_red_traffic_light: + object_margin_to_path: 2.0 occlusion: enable: false @@ -81,6 +83,9 @@ attention_lane_curvature_calculation_ds: 0.5 static_occlusion_with_traffic_light_timeout: 3.5 + debug: + ttc: [0] + enable_rtc: intersection: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval intersection_to_occlusion: false diff --git a/planning/behavior_velocity_intersection_module/scripts/ttc.py b/planning/behavior_velocity_intersection_module/scripts/ttc.py new file mode 100755 index 0000000000000..e4633981570d1 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/scripts/ttc.py @@ -0,0 +1,287 @@ +#!/usr/bin/env python3 + +# Copyright 2023 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. + +import argparse +from dataclasses import dataclass +from itertools import cycle +import math +from threading import Lock +import time + +import imageio +import matplotlib +import matplotlib.pyplot as plt +import numpy as np +import rclpy +from rclpy.node import Node +from tier4_debug_msgs.msg import Float64MultiArrayStamped + +matplotlib.use("TKAgg") + + +@dataclass +class NPC: + x: float + y: float + th: float + width: float + height: float + speed: float + dangerous: bool + ref_object_enter_time: float + ref_object_exit_time: float + collision_start_time: float + collision_start_dist: float + collision_end_time: float + collision_end_dist: float + pred_x: list[float] + pred_y: list[float] + + def __init__(self, data: list[float]): + self.x = data[0] + self.y = data[1] + self.th = data[2] + self.width = data[3] + self.height = data[4] + self.speed = data[5] + self.dangerous = bool(int(data[6])) + self.ref_object_enter_time = data[7] + self.ref_object_exit_time = data[8] + self.collision_start_time = data[9] + self.collision_start_dist = data[10] + self.collision_end_time = data[11] + self.collision_end_dist = data[12] + self.first_collision_x = data[13] + self.first_collision_y = data[14] + self.last_collision_x = data[15] + self.last_collision_y = data[16] + self.pred_x = data[17:58:2] + self.pred_y = data[18:58:2] + + +class TTCVisualizer(Node): + def __init__(self, args): + super().__init__("ttc_visualizer") + self.ttc_dist_pub = self.create_subscription( + Float64MultiArrayStamped, + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection/ego_ttc", + self.on_ego_ttc, + 1, + ) + self.ttc_time_pub = self.create_subscription( + Float64MultiArrayStamped, + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection/object_ttc", + self.on_object_ttc, + 1, + ) + self.args = args + self.lane_id = args.lane_id + self.ego_ttc_data = None + self.object_ttc_data = None + self.npc_vehicles = [] + self.images = [] + self.last_sub = time.time() + + self.plot_timer = self.create_timer(0.2, self.on_plot_timer) + self.fig = plt.figure(figsize=(13, 6)) + self.ttc_ax = self.fig.add_subplot(1, 2, 1) + self.ttc_vel_ax = self.ttc_ax.twinx() + self.world_ax = self.fig.add_subplot(1, 2, 2) + self.lock = Lock() + self.color_list = [ + "#e41a1c", + "#377eb8", + "#4daf4a", + "#984ea3", + "#ff7f00", + "#ffff33", + "#a65628", + "#f781bf", + ] + plt.ion() + plt.show(block=False) + + def plot_ttc(self): + self.ttc_ax.cla() + self.ttc_vel_ax.cla() + + n_ttc_data = int(self.ego_ttc_data.layout.dim[1].size) + ego_ttc_time = self.ego_ttc_data.data[n_ttc_data : 2 * n_ttc_data] + ego_ttc_dist = self.ego_ttc_data.data[2 * n_ttc_data : 3 * n_ttc_data] + + self.ttc_ax.grid() + self.ttc_ax.set_xlabel("ego time") + self.ttc_ax.set_ylabel("ego dist") + time_dist_plot = self.ttc_ax.plot(ego_ttc_time, ego_ttc_dist, label="time-dist", c="orange") + self.ttc_ax.set_xlim(min(ego_ttc_time) - 2.0, max(ego_ttc_time) + 3.0) + self.ttc_ax.set_ylim(min(ego_ttc_dist) - 2.0, max(ego_ttc_dist) + 3.0) + for npc, color in zip(self.npc_vehicles, cycle(self.color_list)): + t0, t1 = npc.collision_start_time, npc.collision_end_time + d0, d1 = npc.collision_start_dist, npc.collision_end_dist + self.ttc_ax.fill( + [t0, t0, t1, t1, 0, 0], + [d0, 0, 0, d1, d1, d0], + c=color, + alpha=0.2, + ) + + dd = [d1 - d0 for d0, d1 in zip(ego_ttc_dist, ego_ttc_dist[1:])] + dt = [t1 - t0 for t0, t1 in zip(ego_ttc_time, ego_ttc_time[1:])] + v = [d / t for d, t in zip(dd, dt)] + self.ttc_vel_ax.yaxis.set_label_position("right") + self.ttc_vel_ax.set_ylabel("ego velocity") + self.ttc_vel_ax.set_ylim(0.0, max(v) + 1.0) + time_velocity_plot = self.ttc_vel_ax.plot(ego_ttc_time[1:], v, label="time-v", c="red") + + lines = time_dist_plot + time_velocity_plot + labels = [line.get_label() for line in lines] + self.ttc_ax.legend(lines, labels, loc="upper left") + + def plot_world(self): + detect_range = self.args.range + self.world_ax.cla() + n_ttc_data = int(self.ego_ttc_data.layout.dim[1].size) + ego_path_x = self.ego_ttc_data.data[3 * n_ttc_data : 4 * n_ttc_data] + ego_path_y = self.ego_ttc_data.data[4 * n_ttc_data : 5 * n_ttc_data] + self.world_ax.set_aspect("equal") + self.world_ax.scatter(ego_path_x[0], ego_path_y[0], marker="x", c="red", s=15) + min_x, max_x = min(ego_path_x), max(ego_path_x) + min_y, max_y = min(ego_path_y), max(ego_path_y) + x_dir = 1 if ego_path_x[-1] > ego_path_x[0] else -1 + y_dir = 1 if ego_path_y[-1] > ego_path_y[0] else -1 + min_x = min_x - detect_range if x_dir == 1 else min_x - 20 + max_x = max_x + detect_range if x_dir == -1 else max_x + 20 + min_y = min_y - detect_range if y_dir == 1 else min_y - 20 + max_y = max_y + detect_range if y_dir == -1 else max_y + 20 + self.world_ax.set_xlim(min_x, max_x) + self.world_ax.set_ylim(min_y, max_y) + arrows = [ + (x0, y0, math.atan2(x1 - x0, y1 - y0)) + for (x0, y0, x1, y1) in zip( + ego_path_x[0:-1:5], + ego_path_y[0:-1:5], + ego_path_x[4:-1:5], + ego_path_y[4:-1:5], + ) + ] + for x, y, th in arrows: + self.world_ax.arrow( + x, + y, + math.sin(th) * 0.5, + math.cos(th) * 0.5, + head_width=0.1, + head_length=0.1, + ) + + for npc, color in zip(self.npc_vehicles, cycle(self.color_list)): + x, y, th, w, h = npc.x, npc.y, npc.th, npc.width, npc.height + bbox = np.array( + [ + [-w / 2, -h / 2], + [+w / 2, -h / 2], + [+w / 2, +h / 2], + [-w / 2, +h / 2], + [-w / 2, -h / 2], + ] + ).transpose() + Rth = np.array([[math.cos(th), -math.sin(th)], [math.sin(th), math.cos(th)]]) + bbox_rot = Rth @ bbox + self.world_ax.fill(bbox_rot[0, :] + x, bbox_rot[1, :] + y, color, alpha=0.5) + self.world_ax.plot( + [npc.first_collision_x, npc.last_collision_x], + [npc.first_collision_y, npc.last_collision_y], + c=color, + linewidth=3.0, + ) + if npc.dangerous: + self.world_ax.plot(npc.pred_x, npc.pred_y, c=color, linewidth=1.5) + else: + self.world_ax.plot(npc.pred_x, npc.pred_y, c=color, linestyle="--") + + self.world_ax.plot(ego_path_x, ego_path_y, c="k", linestyle="--") + + def cleanup(self): + if self.args.save: + kwargs_write = {"fps": self.args.fps, "quantizer": "nq"} + imageio.mimsave("./" + self.args.gif + ".gif", self.images, **kwargs_write) + + def on_plot_timer(self): + with self.lock: + if (not self.ego_ttc_data) or (not self.object_ttc_data): + return + + if not self.last_sub: + return + + now = time.time() + if (now - self.last_sub) > 1.0: + print("elapsed more than 1sec from last sub, exit/save fig") + self.cleanup() + + self.plot_ttc() + self.plot_world() + self.fig.canvas.flush_events() + + if self.args.save: + image = np.frombuffer(self.fig.canvas.tostring_rgb(), dtype="uint8") + image = image.reshape(self.fig.canvas.get_width_height()[::-1] + (3,)) + self.images.append(image) + + def on_ego_ttc(self, msg): + with self.lock: + if int(msg.data[0]) == self.lane_id: + self.ego_ttc_data = msg + self.last_sub = time.time() + + def parse_npc_vehicles(self): + self.npc_vehicles = [] + n_npc_vehicles = int(self.object_ttc_data.layout.dim[0].size) + npc_data_size = int(self.object_ttc_data.layout.dim[1].size) + for i in range(1, n_npc_vehicles): + data = self.object_ttc_data.data[i * npc_data_size : (i + 1) * npc_data_size] + self.npc_vehicles.append(NPC(data)) + + def on_object_ttc(self, msg): + with self.lock: + if int(msg.data[0]) == self.lane_id: + self.object_ttc_data = msg + self.parse_npc_vehicles() + self.last_sub = time.time() + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument( + "--lane_id", + type=int, + required=True, + help="lane_id to analyze", + ) + parser.add_argument( + "--range", + type=float, + default=60, + help="detect range for drawing", + ) + parser.add_argument("-s", "--save", action="store_true", help="flag to save gif") + parser.add_argument("--gif", type=str, default="ttc", help="filename of gif file") + parser.add_argument("--fps", type=float, default=5, help="fps of gif") + args = parser.parse_args() + + rclpy.init() + visualizer = TTCVisualizer(args) + rclpy.spin(visualizer) diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index f472c4bf51e31..8d9beb34d05ee 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -230,6 +230,12 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( debug_data_.amber_ignore_targets, "amber_ignore_targets", module_id_, now, 0.0, 1.0, 0.0), &debug_marker_array, now); + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.red_overshoot_ignore_targets, "red_overshoot_ignore_targets", module_id_, now, + 0.0, 1.0, 0.0), + &debug_marker_array, now); + appendMarkerArray( debug::createObjectsMarkerArray( debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2), diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index a65e06eedcdf0..5c5f5eac16b81 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -139,6 +139,9 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration = getOrDeclareParameter( node, ns + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration"); + ip.collision_detection.ignore_on_red_traffic_light.object_margin_to_path = + getOrDeclareParameter( + node, ns + ".collision_detection.ignore_on_red_traffic_light.object_margin_to_path"); ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = @@ -178,6 +181,7 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.attention_lane_curvature_calculation_ds"); ip.occlusion.static_occlusion_with_traffic_light_timeout = getOrDeclareParameter( node, ns + ".occlusion.static_occlusion_with_traffic_light_timeout"); + ip.debug.ttc = getOrDeclareParameter>(node, ns + ".debug.ttc"); } void IntersectionModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 4def152567b32..c3b429680ab91 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -30,6 +30,7 @@ #include #include +#include #include #include #include @@ -137,6 +138,10 @@ IntersectionModule::IntersectionModule( decision_state_pub_ = node_.create_publisher("~/debug/intersection/decision_state", 1); + ego_ttc_pub_ = node_.create_publisher( + "~/debug/intersection/ego_ttc", 1); + object_ttc_pub_ = node_.create_publisher( + "~/debug/intersection/object_ttc", 1); } void IntersectionModule::initializeRTCStatus() @@ -1063,16 +1068,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool yield_stuck_detected = checkYieldStuckVehicle( planner_data_, path_lanelets, intersection_lanelets.first_attention_area()); if (yield_stuck_detected && stuck_stop_line_idx_opt) { - auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); - if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) { - if ( - default_stop_line_idx_opt && - fromEgoDist(stuck_stop_line_idx) < -planner_param_.common.stop_overshoot_margin) { - stuck_stop_line_idx = default_stop_line_idx_opt.value(); - } - } else { - return IntersectionModule::YieldStuckStop{closest_idx, stuck_stop_line_idx}; - } + const auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); + return IntersectionModule::YieldStuckStop{closest_idx, stuck_stop_line_idx}; } // if attention area is empty, collision/occlusion detection is impossible @@ -1472,12 +1469,22 @@ bool IntersectionModule::checkCollision( // check collision between target_objects predicted path and ego lane // cut the predicted path at passing_time + tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; const auto time_distance_array = util::calcIntersectionPassingTime( - path, planner_data_, associative_ids_, closest_idx, last_intersection_stop_line_candidate_idx, - time_delay, planner_param_.common.intersection_velocity, + path, planner_data_, lane_id_, associative_ids_, closest_idx, + last_intersection_stop_line_candidate_idx, time_delay, + planner_param_.common.intersection_velocity, planner_param_.collision_detection.minimum_ego_predicted_velocity, planner_param_.collision_detection.use_upstream_velocity, - planner_param_.collision_detection.minimum_upstream_velocity); + planner_param_.collision_detection.minimum_upstream_velocity, &ego_ttc_time_array); + + if ( + std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != + planner_param_.debug.ttc.end()) { + ego_ttc_time_array.stamp = path.header.stamp; + ego_ttc_pub_->publish(ego_ttc_time_array); + } + const double passing_time = time_distance_array.back().first; util::cutPredictPathWithDuration(target_objects, clock_, passing_time); @@ -1519,18 +1526,77 @@ bool IntersectionModule::checkCollision( .object_expected_deceleration)); return dist_to_stop_line > braking_distance; }; - + const auto isTolerableOvershoot = [&](const util::TargetObject & target_object) { + if ( + !target_object.attention_lanelet || !target_object.dist_to_stop_line || + !target_object.stop_line) { + return false; + } + const double dist_to_stop_line = target_object.dist_to_stop_line.value(); + const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = + v * v / + (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration)); + if (dist_to_stop_line > braking_distance) { + return false; + } + const auto stopline_front = target_object.stop_line.value().front(); + const auto stopline_back = target_object.stop_line.value().back(); + tier4_autoware_utils::LineString2d object_line; + object_line.emplace_back( + (stopline_front.x() + stopline_back.x()) / 2.0, + (stopline_front.y() + stopline_back.y()) / 2.0); + const auto stopline_mid = object_line.front(); + const auto endpoint = target_object.attention_lanelet.value().centerline().back(); + object_line.emplace_back(endpoint.x(), endpoint.y()); + std::vector intersections; + bg::intersection(object_line, ego_lane.centerline2d().basicLineString(), intersections); + if (intersections.empty()) { + return false; + } + const auto collision_point = intersections.front(); + // distance from object expected stop position to collision point + const double stopline_to_object = -1.0 * dist_to_stop_line + braking_distance; + const double stopline_to_collision = + std::hypot(collision_point.x() - stopline_mid.x(), collision_point.y() - stopline_mid.y()); + const double object2collision = stopline_to_collision - stopline_to_object; + const double margin = + planner_param_.collision_detection.ignore_on_red_traffic_light.object_margin_to_path; + return (object2collision > margin) || (object2collision < 0); + }; // check collision between predicted_path and ego_area + tier4_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array; + object_ttc_time_array.layout.dim.resize(3); + object_ttc_time_array.layout.dim.at(0).label = "objects"; + object_ttc_time_array.layout.dim.at(0).size = 1; // incremented in the loop, first row is lane_id + object_ttc_time_array.layout.dim.at(1).label = + "[x, y, th, length, width, speed, dangerous, ref_obj_enter_time, ref_obj_exit_time, " + "start_time, start_dist, " + "end_time, end_dist, first_collision_x, first_collision_y, last_collision_x, last_collision_y, " + "prd_x[0], ... pred_x[19], pred_y[0], ... pred_y[19]]"; + object_ttc_time_array.layout.dim.at(1).size = 57; + for (unsigned i = 0; i < object_ttc_time_array.layout.dim.at(1).size; ++i) { + object_ttc_time_array.data.push_back(lane_id_); + } bool collision_detected = false; for (const auto & target_object : target_objects->all_attention_objects) { const auto & object = target_object.object; // If the vehicle is expected to stop before their stopline, ignore + const bool expected_to_stop_before_stopline = expectedToStopBeforeStopLine(target_object); if ( traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && - expectedToStopBeforeStopLine(target_object)) { + expected_to_stop_before_stopline) { debug_data_.amber_ignore_targets.objects.push_back(object); continue; } + const bool is_tolerable_overshoot = isTolerableOvershoot(target_object); + if ( + traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED && + !expected_to_stop_before_stopline && is_tolerable_overshoot) { + debug_data_.red_overshoot_ignore_targets.objects.push_back(object); + continue; + } for (const auto & predicted_path : object.kinematics.predicted_paths) { if ( predicted_path.confidence < @@ -1611,6 +1677,24 @@ bool IntersectionModule::checkCollision( break; } } + object_ttc_time_array.layout.dim.at(0).size++; + const auto & pos = object.kinematics.initial_pose_with_covariance.pose.position; + const auto & shape = object.shape; + object_ttc_time_array.data.insert( + object_ttc_time_array.data.end(), + {pos.x, pos.y, tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation), + shape.dimensions.x, shape.dimensions.y, + object.kinematics.initial_twist_with_covariance.twist.linear.x, + 1.0 * static_cast(collision_detected), ref_object_enter_time, ref_object_exit_time, + start_time_distance_itr->first, start_time_distance_itr->second, + end_time_distance_itr->first, end_time_distance_itr->second, first_itr->position.x, + first_itr->position.y, last_itr->position.x, last_itr->position.y}); + for (unsigned i = 0; i < 20; i++) { + const auto & pos = + predicted_path.path.at(std::min(i, predicted_path.path.size() - 1)).position; + object_ttc_time_array.data.push_back(pos.x); + object_ttc_time_array.data.push_back(pos.y); + } if (collision_detected) { debug_data_.conflicting_targets.objects.push_back(object); break; @@ -1618,6 +1702,13 @@ bool IntersectionModule::checkCollision( } } + if ( + std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != + planner_param_.debug.ttc.end()) { + object_ttc_time_array.stamp = path.header.stamp; + object_ttc_pub_->publish(object_ttc_time_array); + } + return collision_detected; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 764f5bd7fe058..c794ef6c53aad 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -118,6 +119,10 @@ class IntersectionModule : public SceneModuleInterface { double object_expected_deceleration; } ignore_on_amber_traffic_light; + struct IgnoreOnRedTrafficLight + { + double object_margin_to_path; + } ignore_on_red_traffic_light; } collision_detection; struct Occlusion { @@ -146,6 +151,10 @@ class IntersectionModule : public SceneModuleInterface double attention_lane_curvature_calculation_ds; double static_occlusion_with_traffic_light_timeout; } occlusion; + struct Debug + { + std::vector ttc; + } debug; }; enum OcclusionType { @@ -359,6 +368,8 @@ class IntersectionModule : public SceneModuleInterface util::DebugData debug_data_; rclcpp::Publisher::SharedPtr decision_state_pub_; + rclcpp::Publisher::SharedPtr ego_ttc_pub_; + rclcpp::Publisher::SharedPtr object_ttc_pub_; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 42412bbccd424..94bc75b805934 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -84,21 +84,16 @@ static std::optional getDuplicatedPointIdx( static std::optional insertPointIndex( const geometry_msgs::msg::Pose & in_pose, - autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path) + autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold) { const auto duplicate_idx_opt = getDuplicatedPointIdx(*inout_path, in_pose.position); if (duplicate_idx_opt) { return duplicate_idx_opt.value(); } - static constexpr double dist_thr = 10.0; - static constexpr double angle_thr = M_PI / 1.5; - const auto closest_idx_opt = - motion_utils::findNearestIndex(inout_path->points, in_pose, dist_thr, angle_thr); - if (!closest_idx_opt) { - return std::nullopt; - } - const size_t closest_idx = closest_idx_opt.get(); + const size_t closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + inout_path->points, in_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); // vector.insert(i) inserts element on the left side of v[i] // the velocity need to be zero order hold(from prior point) int insert_idx = closest_idx; @@ -165,7 +160,7 @@ std::optional> findLaneIdsInterval( */ static std::optional getStopLineIndexFromMap( const InterpolatedPathInfo & interpolated_path_info, - const std::shared_ptr & planner_data, const double dist_thr) + const std::shared_ptr & planner_data) { const auto & path = interpolated_path_info.path; const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); @@ -212,12 +207,9 @@ static std::optional getStopLineIndexFromMap( stop_point_from_map.position.y = 0.5 * (p_start.y() + p_end.y()); stop_point_from_map.position.z = 0.5 * (p_start.z() + p_end.z()); - const auto stop_idx_ip_opt = - motion_utils::findNearestIndex(path.points, stop_point_from_map, static_cast(dist_thr)); - if (!stop_idx_ip_opt) { - return std::nullopt; - } - return stop_idx_ip_opt.get(); + return motion_utils::findFirstNearestIndexWithSoftConstraints( + path.points, stop_point_from_map, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); } static std::optional getFirstPointInsidePolygonByFootprint( @@ -304,8 +296,7 @@ std::optional generateIntersectionStopLines( // (1) default stop line position on interpolated path bool default_stop_line_valid = true; int stop_idx_ip_int = -1; - if (const auto map_stop_idx_ip = - getStopLineIndexFromMap(interpolated_path_info, planner_data, 10.0); + if (const auto map_stop_idx_ip = getStopLineIndexFromMap(interpolated_path_info, planner_data); map_stop_idx_ip) { stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; } @@ -319,12 +310,9 @@ std::optional generateIntersectionStopLines( // (2) ego front stop line position on interpolated path const geometry_msgs::msg::Pose & current_pose = planner_data->current_odometry->pose; - const auto closest_idx_ip_opt = - motion_utils::findNearestIndex(path_ip.points, current_pose, 3.0, M_PI_4); - if (!closest_idx_ip_opt) { - return std::nullopt; - } - const auto closest_idx_ip = closest_idx_ip_opt.value(); + const auto closest_idx_ip = motion_utils::findFirstNearestIndexWithSoftConstraints( + path_ip.points, current_pose, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); // (3) occlusion peeking stop line position on interpolated path int occlusion_peeking_line_ip_int = static_cast(default_stop_line_ip); @@ -404,7 +392,9 @@ std::optional generateIntersectionStopLines( [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); for (const auto & [stop_idx_ip, stop_idx] : stop_lines) { const auto & insert_point = path_ip.points.at(*stop_idx_ip).point.pose; - const auto insert_idx = insertPointIndex(insert_point, original_path); + const auto insert_idx = insertPointIndex( + insert_point, original_path, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); if (!insert_idx) { return std::nullopt; } @@ -555,7 +545,9 @@ std::optional generateStuckStopLine( static_cast(stuck_stop_line_idx_ip) - 1 - stop_line_margin_idx_dist - base2front_idx_dist, 0)); const auto & insert_point = path_ip.points.at(insert_idx_ip).point.pose; - return insertPointIndex(insert_point, original_path); + return insertPointIndex( + insert_point, original_path, planner_data->ego_nearest_dist_threshold, + planner_data->ego_nearest_yaw_threshold); } static std::vector getPolygon3dFromLanelets( @@ -841,9 +833,16 @@ IntersectionLanelets getObjectiveLanelets( result.attention_stop_lines_.push_back(stop_line); } result.attention_non_preceding_ = std::move(detection_lanelets); - // TODO(Mamoru Sobue): find stop lines for attention_non_preceding_ if needed for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { - result.attention_non_preceding_stop_lines_.push_back(std::nullopt); + std::optional stop_line = std::nullopt; + const auto & ll = result.attention_non_preceding_.at(i); + const auto traffic_lights = ll.regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stop_line_opt = traffic_light->stopLine(); + if (!stop_line_opt) continue; + stop_line = stop_line_opt.get(); + } + result.attention_non_preceding_stop_lines_.push_back(stop_line); } result.conflicting_ = std::move(conflicting_ex_ego_lanelets); result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids); @@ -1298,10 +1297,12 @@ void cutPredictPathWithDuration( TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::shared_ptr & planner_data, const std::set & associative_ids, - const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, - const double time_delay, const double intersection_velocity, const double minimum_ego_velocity, - const bool use_upstream_velocity, const double minimum_upstream_velocity) + const std::shared_ptr & planner_data, const lanelet::Id lane_id, + const std::set & associative_ids, const size_t closest_idx, + const size_t last_intersection_stop_line_candidate_idx, const double time_delay, + const double intersection_velocity, const double minimum_ego_velocity, + const bool use_upstream_velocity, const double minimum_upstream_velocity, + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) { double dist_sum = 0.0; int assigned_lane_found = false; @@ -1339,13 +1340,10 @@ TimeDistanceArray calcIntersectionPassingTime( // `last_intersection_stop_line_candidate_idx` makes no sense const auto last_intersection_stop_line_candidate_point_orig = path.points.at(last_intersection_stop_line_candidate_idx).point.pose; - const auto last_intersection_stop_line_candidate_nearest_ind_opt = motion_utils::findNearestIndex( - smoothed_reference_path.points, last_intersection_stop_line_candidate_point_orig, 3.0, M_PI_4); - if (!last_intersection_stop_line_candidate_nearest_ind_opt) { - return time_distance_array; - } const auto last_intersection_stop_line_candidate_nearest_ind = - last_intersection_stop_line_candidate_nearest_ind_opt.value(); + motion_utils::findFirstNearestIndexWithSoftConstraints( + smoothed_reference_path.points, last_intersection_stop_line_candidate_point_orig, + planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); for (size_t i = 1; i < smoothed_reference_path.points.size(); ++i) { const auto & p1 = smoothed_reference_path.points.at(i - 1); @@ -1367,7 +1365,26 @@ TimeDistanceArray calcIntersectionPassingTime( time_distance_array.emplace_back(passing_time, dist_sum); } - + debug_ttc_array->layout.dim.resize(3); + debug_ttc_array->layout.dim.at(0).label = "lane_id_@[0][0], ttc_time, ttc_dist, path_x, path_y"; + debug_ttc_array->layout.dim.at(0).size = 5; + debug_ttc_array->layout.dim.at(1).label = "values"; + debug_ttc_array->layout.dim.at(1).size = time_distance_array.size(); + for (unsigned i = 0; i < time_distance_array.size(); ++i) { + debug_ttc_array->data.push_back(lane_id); + } + for (const auto & [t, d] : time_distance_array) { + debug_ttc_array->data.push_back(t); + } + for (const auto & [t, d] : time_distance_array) { + debug_ttc_array->data.push_back(d); + } + for (const auto & p : smoothed_reference_path.points) { + debug_ttc_array->data.push_back(p.point.pose.position.x); + } + for (const auto & p : smoothed_reference_path.points) { + debug_ttc_array->data.push_back(p.point.pose.position.y); + } return time_distance_array; } @@ -1435,6 +1452,37 @@ static lanelet::ConstLanelets getPrevLanelets( return previous_lanelets; } +// end inclusive +lanelet::ConstLanelet generatePathLanelet( + const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, + const double interval) +{ + lanelet::Points3d lefts; + lanelet::Points3d rights; + for (size_t i = start_idx; i <= end_idx; ++i) { + const auto & p = path.points.at(i).point.pose; + if (start_idx < i && i != end_idx) { + const auto & p_prev = path.points.at(i - 1).point.pose; + if (tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) { + continue; + } + } + const double yaw = tf2::getYaw(p.orientation); + const double x = p.position.x; + const double y = p.position.y; + const double left_x = x + width / 2 * std::sin(yaw); + const double left_y = y - width / 2 * std::cos(yaw); + const double right_x = x - width / 2 * std::sin(yaw); + const double right_y = y + width / 2 * std::cos(yaw); + lefts.emplace_back(lanelet::InvalId, left_x, left_y, p.position.z); + rights.emplace_back(lanelet::InvalId, right_x, right_y, p.position.z); + } + lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, lefts); + lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, rights); + + return lanelet::Lanelet(lanelet::InvalId, left, right); +} + std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, const util::InterpolatedPathInfo & interpolated_path_info, const std::set & associative_ids, @@ -1444,6 +1492,8 @@ std::optional generatePathLanelets( const std::vector & attention_areas, const size_t closest_idx, const double width) { + static constexpr double path_lanelet_interval = 1.5; + const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; if (!assigned_lane_interval_opt) { return std::nullopt; @@ -1460,13 +1510,13 @@ std::optional generatePathLanelets( const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval; if (closest_idx > assigned_lane_start) { path_lanelets.all.push_back( - planning_utils::generatePathLanelet(path, assigned_lane_start, closest_idx, width)); + generatePathLanelet(path, assigned_lane_start, closest_idx, width, path_lanelet_interval)); } // ego_or_entry2exit const auto ego_or_entry_start = std::max(closest_idx, assigned_lane_start); path_lanelets.ego_or_entry2exit = - planning_utils::generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width); + generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width, path_lanelet_interval); path_lanelets.all.push_back(path_lanelets.ego_or_entry2exit); // next @@ -1475,7 +1525,8 @@ std::optional generatePathLanelets( const auto next_lane_interval_opt = findLaneIdsInterval(path, {next_id}); if (next_lane_interval_opt) { const auto [next_start, next_end] = next_lane_interval_opt.value(); - path_lanelets.next = planning_utils::generatePathLanelet(path, next_start, next_end, width); + path_lanelets.next = + generatePathLanelet(path, next_start, next_end, width, path_lanelet_interval); path_lanelets.all.push_back(path_lanelets.next.value()); } } @@ -1491,12 +1542,13 @@ std::optional generatePathLanelets( if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; - lanelet::ConstLanelet conflicting_interval = planning_utils::generatePathLanelet( - path, first_inside_conflicting_idx, last_inside_conflicting_idx, width); + lanelet::ConstLanelet conflicting_interval = generatePathLanelet( + path, first_inside_conflicting_idx, last_inside_conflicting_idx, width, + path_lanelet_interval); path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval)); if (last_inside_conflicting_idx < assigned_lane_end) { - lanelet::ConstLanelet remaining_interval = planning_utils::generatePathLanelet( - path, last_inside_conflicting_idx, assigned_lane_end, width); + lanelet::ConstLanelet remaining_interval = generatePathLanelet( + path, last_inside_conflicting_idx, assigned_lane_end, width, path_lanelet_interval); path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval)); } } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 5faacd4325b06..2f204f06aac7c 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -20,6 +20,8 @@ #include +#include + #include #include #include @@ -146,15 +148,21 @@ void cutPredictPathWithDuration( TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::shared_ptr & planner_data, const std::set & associative_ids, - const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, - const double time_delay, const double intersection_velocity, const double minimum_ego_velocity, - const bool use_upstream_velocity, const double minimum_upstream_velocity); + const std::shared_ptr & planner_data, const lanelet::Id lane_id, + const std::set & associative_ids, const size_t closest_idx, + const size_t last_intersection_stop_line_candidate_idx, const double time_delay, + const double intersection_velocity, const double minimum_ego_velocity, + const bool use_upstream_velocity, const double minimum_upstream_velocity, + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); double calcDistanceUntilIntersectionLanelet( const lanelet::ConstLanelet & assigned_lanelet, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx); +lanelet::ConstLanelet generatePathLanelet( + const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, + const double interval); + std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, const util::InterpolatedPathInfo & interpolated_path_info, const std::set & associative_ids, diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index fdcf05a97a7a9..73e60aea6471a 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -49,6 +49,7 @@ struct DebugData std::vector candidate_collision_object_polygons; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; std::vector occlusion_polygons; diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index d83f74a71b66f..1357b555f0ba4 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -11,7 +11,6 @@ Kyoichi Sugahara Taiki Tanaka Kosuke Takeuchi - Yutaka Shimizu Tomohito Ando Makoto Kurihara Maxime Clement diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp index ceba86c27f864..86273b1e35e1d 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp @@ -100,8 +100,6 @@ geometry_msgs::msg::Pose getAheadPose( const autoware_auto_planning_msgs::msg::PathWithLaneId & path); Polygon2d generatePathPolygon( const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width); -lanelet::ConstLanelet generatePathLanelet( - const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width); double calcJudgeLineDistWithAccLimit( const double velocity, const double max_stop_acceleration, const double delay_response_time); diff --git a/planning/behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner_common/src/utilization/util.cpp index a3a5a8a169759..85ceea0d391cc 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/util.cpp @@ -356,32 +356,6 @@ Polygon2d generatePathPolygon( return ego_area; } -lanelet::ConstLanelet generatePathLanelet( - const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width) -{ - lanelet::Points3d lefts; - for (size_t i = start_idx; i <= end_idx; ++i) { - const double yaw = tf2::getYaw(path.points.at(i).point.pose.orientation); - const double x = path.points.at(i).point.pose.position.x + width / 2 * std::sin(yaw); - const double y = path.points.at(i).point.pose.position.y - width / 2 * std::cos(yaw); - const lanelet::Point3d p(lanelet::InvalId, x, y, path.points.at(i).point.pose.position.z); - lefts.emplace_back(p); - } - lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, lefts); - - lanelet::Points3d rights; - for (size_t i = start_idx; i <= end_idx; ++i) { - const double yaw = tf2::getYaw(path.points.at(i).point.pose.orientation); - const double x = path.points.at(i).point.pose.position.x - width / 2 * std::sin(yaw); - const double y = path.points.at(i).point.pose.position.y + width / 2 * std::cos(yaw); - const lanelet::Point3d p(lanelet::InvalId, x, y, path.points.at(i).point.pose.position.z); - rights.emplace_back(p); - } - lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, rights); - - return lanelet::Lanelet(lanelet::InvalId, left, right); -} - double calcJudgeLineDistWithAccLimit( const double velocity, const double max_stop_acceleration, const double delay_response_time) { diff --git a/planning/behavior_velocity_stop_line_module/package.xml b/planning/behavior_velocity_stop_line_module/package.xml index 8610b7b428bec..ee79d64312161 100644 --- a/planning/behavior_velocity_stop_line_module/package.xml +++ b/planning/behavior_velocity_stop_line_module/package.xml @@ -5,7 +5,8 @@ 0.1.0 The behavior_velocity_stop_line_module package - Yutaka Shimizu + Fumiya Watanabe + Zhe Shen Tomoya Kimura Shumpei Wakabayashi diff --git a/planning/costmap_generator/package.xml b/planning/costmap_generator/package.xml index feed19fdcc235..ae124a1774881 100644 --- a/planning/costmap_generator/package.xml +++ b/planning/costmap_generator/package.xml @@ -7,7 +7,6 @@ Kosuke Takeuchi Takamasa Horibe Takayuki Murooka - Yutaka Shimizu Apache License 2.0 diff --git a/planning/freespace_planner/package.xml b/planning/freespace_planner/package.xml index f92c0a449970d..22557f8a0bbb3 100644 --- a/planning/freespace_planner/package.xml +++ b/planning/freespace_planner/package.xml @@ -7,7 +7,6 @@ Kosuke Takeuchi Takamasa Horibe Takayuki Murooka - Yutaka Shimizu Apache License 2.0 Akihito OHSATO diff --git a/planning/freespace_planning_algorithms/package.xml b/planning/freespace_planning_algorithms/package.xml index 372e623e101de..2b20a935c4ae1 100644 --- a/planning/freespace_planning_algorithms/package.xml +++ b/planning/freespace_planning_algorithms/package.xml @@ -7,7 +7,6 @@ Kosuke Takeuchi Takamasa Horibe Takayuki Murooka - Yutaka Shimizu Apache License 2.0 Akihito Ohsato diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index 49e1d0de8be93..4fcf2661ad20e 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -9,7 +9,6 @@ Ryohsuke Mitsudome Takamasa Horibe Takayuki Murooka - Yutaka Shimizu Apache License 2.0 Ryohsuke Mitsudome diff --git a/planning/motion_velocity_smoother/package.xml b/planning/motion_velocity_smoother/package.xml index f19ead1874f21..54c60c938970a 100644 --- a/planning/motion_velocity_smoother/package.xml +++ b/planning/motion_velocity_smoother/package.xml @@ -7,7 +7,6 @@ Fumiya Watanabe Takamasa Horibe - Yutaka Shimizu Makoto Kurihara Apache License 2.0 diff --git a/planning/obstacle_cruise_planner/package.xml b/planning/obstacle_cruise_planner/package.xml index f482be8ebafe7..e724ddb3e6bd7 100644 --- a/planning/obstacle_cruise_planner/package.xml +++ b/planning/obstacle_cruise_planner/package.xml @@ -5,7 +5,6 @@ The obstacle_cruise_planner package Takayuki Murooka - Yutaka Shimizu Kosuke Takeuchi Satoshi Ota diff --git a/planning/planning_topic_converter/CMakeLists.txt b/planning/planning_topic_converter/CMakeLists.txt new file mode 100644 index 0000000000000..75b63d26c316f --- /dev/null +++ b/planning/planning_topic_converter/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.5) +project(planning_topic_converter) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_library(planning_topic_converter SHARED + src/path_to_trajectory.cpp +) + +rclcpp_components_register_node(planning_topic_converter + PLUGIN "planning_topic_converter::PathToTrajectory" + EXECUTABLE path_to_trajectory_converter +) + +ament_auto_package() diff --git a/planning/planning_topic_converter/README.md b/planning/planning_topic_converter/README.md new file mode 100644 index 0000000000000..7be6979e62281 --- /dev/null +++ b/planning/planning_topic_converter/README.md @@ -0,0 +1,34 @@ +# Planning Topic Converter + +## Purpose + +This package provides tools that convert topic type among types are defined in . + +## Inner-workings / Algorithms + +### Usage example + +The tools in this package are provided as composable ROS 2 component nodes, so that they can be spawned into an existing process, launched from launch files, or invoked from the command line. + +```xml + + + + + + + + + +``` + +## Parameters + +| Name | Type | Description | +| :------------- | :----- | :----------------- | +| `input_topic` | string | input topic name. | +| `output_topic` | string | output topic name. | + +## Assumptions / Known limits + +## Future extensions / Unimplemented parts diff --git a/planning/planning_topic_converter/include/planning_topic_converter/converter_base.hpp b/planning/planning_topic_converter/include/planning_topic_converter/converter_base.hpp new file mode 100644 index 0000000000000..a093d9a952029 --- /dev/null +++ b/planning/planning_topic_converter/include/planning_topic_converter/converter_base.hpp @@ -0,0 +1,51 @@ +// Copyright 2023 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 PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ +#define PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include +#include + +namespace planning_topic_converter +{ + +template +class ConverterBase : public rclcpp::Node +{ +public: + ConverterBase(const std::string & node_name, const rclcpp::NodeOptions & options) + : rclcpp::Node(node_name, options) + { + const auto input_topic = this->declare_parameter("input_topic"); + const auto output_topic = this->declare_parameter("output_topic"); + + pub_ = this->create_publisher(output_topic, 1); + sub_ = this->create_subscription( + input_topic, 1, std::bind(&ConverterBase::process, this, std::placeholders::_1)); + } + +protected: + virtual void process(const typename InputType::ConstSharedPtr msg) = 0; + typename rclcpp::Publisher::SharedPtr pub_; + typename rclcpp::Subscription::SharedPtr sub_; + +private: +}; + +} // namespace planning_topic_converter + +#endif // PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ diff --git a/planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp b/planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp new file mode 100644 index 0000000000000..f214f2a4d5e2a --- /dev/null +++ b/planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp @@ -0,0 +1,45 @@ +// Copyright 2023 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 PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ +#define PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ + +#include "planning_topic_converter/converter_base.hpp" +#include "rclcpp/rclcpp.hpp" + +#include +#include + +#include + +namespace planning_topic_converter +{ + +using autoware_auto_planning_msgs::msg::Path; +using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; + +class PathToTrajectory : public ConverterBase +{ +public: + explicit PathToTrajectory(const rclcpp::NodeOptions & options); + +private: + void process(const Path::ConstSharedPtr msg) override; +}; + +} // namespace planning_topic_converter + +#endif // PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ diff --git a/planning/planning_topic_converter/package.xml b/planning/planning_topic_converter/package.xml new file mode 100644 index 0000000000000..d07d59f3d2c89 --- /dev/null +++ b/planning/planning_topic_converter/package.xml @@ -0,0 +1,29 @@ + + + planning_topic_converter + 0.1.0 + The planning_topic_converter package + + Satoshi OTA + Shumpei Wakabayashi + Kosuke Takeuchi + + Apache License 2.0 + + Satoshi OTA + + ament_cmake_auto + + autoware_auto_planning_msgs + motion_utils + rclcpp + rclcpp_components + tier4_autoware_utils + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/planning_topic_converter/src/path_to_trajectory.cpp b/planning/planning_topic_converter/src/path_to_trajectory.cpp new file mode 100644 index 0000000000000..5362686617fc7 --- /dev/null +++ b/planning/planning_topic_converter/src/path_to_trajectory.cpp @@ -0,0 +1,69 @@ +// Copyright 2023 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 "planning_topic_converter/path_to_trajectory.hpp" + +#include +#include + +namespace planning_topic_converter +{ +namespace +{ +TrajectoryPoint convertToTrajectoryPoint(const PathPoint & point) +{ + TrajectoryPoint traj_point; + traj_point.pose = tier4_autoware_utils::getPose(point); + traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; + traj_point.lateral_velocity_mps = point.lateral_velocity_mps; + traj_point.heading_rate_rps = point.heading_rate_rps; + return traj_point; +} + +std::vector convertToTrajectoryPoints(const std::vector & points) +{ + std::vector traj_points; + for (const auto & point : points) { + const auto traj_point = convertToTrajectoryPoint(point); + traj_points.push_back(traj_point); + } + return traj_points; +} + +Trajectory createTrajectory( + const std_msgs::msg::Header & header, const std::vector & trajectory_points) +{ + auto trajectory = motion_utils::convertToTrajectory(trajectory_points); + trajectory.header = header; + + return trajectory; +} +} // namespace + +PathToTrajectory::PathToTrajectory(const rclcpp::NodeOptions & options) +: ConverterBase("path_to_trajectory_converter", options) +{ +} + +void PathToTrajectory::process(const Path::ConstSharedPtr msg) +{ + const auto trajectory_points = convertToTrajectoryPoints(msg->points); + const auto output = createTrajectory(msg->header, trajectory_points); + pub_->publish(output); +} + +} // namespace planning_topic_converter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(planning_topic_converter::PathToTrajectory) diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/planning_validator/planning_validator.hpp index ef570b57773bb..037c7fb0f4e95 100644 --- a/planning/planning_validator/include/planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/planning_validator/planning_validator.hpp @@ -109,7 +109,7 @@ class PlanningValidator : public rclcpp::Node int diag_error_count_threshold_ = 0; bool display_on_terminal_ = true; - Updater diag_updater_{this}; + std::shared_ptr diag_updater_ = nullptr; PlanningValidatorStatus validation_status_; ValidationParams validation_params_; // for thresholds diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index 875781d47b25d..58af2c08ccb22 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -47,10 +47,6 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options) setupParameters(); - if (publish_diag_) { - setupDiag(); - } - logger_configure_ = std::make_unique(this); } @@ -112,39 +108,40 @@ void PlanningValidator::setStatus( void PlanningValidator::setupDiag() { + diag_updater_ = std::make_shared(this); auto & d = diag_updater_; - d.setHardwareID("planning_validator"); + d->setHardwareID("planning_validator"); std::string ns = "trajectory_validation_"; - d.add(ns + "finite", [&](auto & stat) { + d->add(ns + "finite", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_finite_value, "infinite value is found"); }); - d.add(ns + "interval", [&](auto & stat) { + d->add(ns + "interval", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_interval, "points interval is too long"); }); - d.add(ns + "relative_angle", [&](auto & stat) { + d->add(ns + "relative_angle", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_relative_angle, "relative angle is too large"); }); - d.add(ns + "curvature", [&](auto & stat) { + d->add(ns + "curvature", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_curvature, "curvature is too large"); }); - d.add(ns + "lateral_acceleration", [&](auto & stat) { + d->add(ns + "lateral_acceleration", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_lateral_acc, "lateral acceleration is too large"); }); - d.add(ns + "acceleration", [&](auto & stat) { + d->add(ns + "acceleration", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_longitudinal_max_acc, "acceleration is too large"); }); - d.add(ns + "deceleration", [&](auto & stat) { + d->add(ns + "deceleration", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_longitudinal_min_acc, "deceleration is too large"); }); - d.add(ns + "steering", [&](auto & stat) { + d->add(ns + "steering", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_steering, "expected steering is too large"); }); - d.add(ns + "steering_rate", [&](auto & stat) { + d->add(ns + "steering_rate", [&](auto & stat) { setStatus( stat, validation_status_.is_valid_steering_rate, "expected steering rate is too large"); }); - d.add(ns + "velocity_deviation", [&](auto & stat) { + d->add(ns + "velocity_deviation", [&](auto & stat) { setStatus( stat, validation_status_.is_valid_velocity_deviation, "velocity deviation is too large"); }); @@ -174,11 +171,15 @@ void PlanningValidator::onTrajectory(const Trajectory::ConstSharedPtr msg) if (!isDataReady()) return; + if (publish_diag_ && !diag_updater_) { + setupDiag(); // run setup after all data is ready. + } + debug_pose_publisher_->clearMarkers(); validate(*current_trajectory_); - diag_updater_.force_update(); + diag_updater_->force_update(); publishTrajectory(); diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml index 29f0411a5aa80..60adfb1531104 100644 --- a/planning/route_handler/package.xml +++ b/planning/route_handler/package.xml @@ -6,7 +6,6 @@ The route_handling package Fumiya Watanabe Zulfaqar Azmi - Yutaka Shimizu Kosuke Takeuchi Takayuki Murooka Mamoru Sobue diff --git a/sensing/radar_static_pointcloud_filter/config/radar_static_pointcloud_filter.param.yaml b/sensing/radar_static_pointcloud_filter/config/radar_static_pointcloud_filter.param.yaml index a429f7a065ffe..01f3a4cdd641a 100644 --- a/sensing/radar_static_pointcloud_filter/config/radar_static_pointcloud_filter.param.yaml +++ b/sensing/radar_static_pointcloud_filter/config/radar_static_pointcloud_filter.param.yaml @@ -1,17 +1,3 @@ -# Copyright 2023 Foxconn, 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. - /**: ros__parameters: doppler_velocity_sd: 4.0 diff --git a/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp index 772dcef753c11..0eda1eae627e7 100644 --- a/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp +++ b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2022-2023 Foxconn, TIER IV, Inc. +// 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. diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index ceecacfe8cd8c..75f77489daada 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -74,6 +74,7 @@ The table below shows which models correspond to what parameters. The model name | vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | 0.25 | [s] | | acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | 0.1 | [s] | | steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | 0.27 | [s] | +| steer_dead_band | double | dead band for steering angle | x | x | x | o | o | o | 0.0 | [rad] | | vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | 0.5 | [s] | | vel_lim | double | limit of velocity | x | x | x | o | o | o | 50.0 | [m/s] | | vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | 7.0 | [m/ss] | diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 4b25dba8b16ee..b0df04285ac9f 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -171,36 +171,36 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node tf2_ros::TransformListener tf_listener_; /* received & published topics */ - PoseWithCovarianceStamped::ConstSharedPtr initial_pose_; - TwistStamped initial_twist_; - VelocityReport current_velocity_; - Odometry current_odometry_; - SteeringReport current_steer_; - AckermannControlCommand current_ackermann_cmd_; - AckermannControlCommand current_manual_ackermann_cmd_; - GearCommand current_gear_cmd_; - GearCommand current_manual_gear_cmd_; - TurnIndicatorsCommand::ConstSharedPtr current_turn_indicators_cmd_ptr_; - HazardLightsCommand::ConstSharedPtr current_hazard_lights_cmd_ptr_; - Trajectory::ConstSharedPtr current_trajectory_ptr_; - bool simulate_motion_; //!< stop vehicle motion simulation if false - ControlModeReport current_control_mode_; - bool enable_road_slope_simulation_; + PoseWithCovarianceStamped::ConstSharedPtr initial_pose_{}; + TwistStamped initial_twist_{}; + VelocityReport current_velocity_{}; + Odometry current_odometry_{}; + SteeringReport current_steer_{}; + AckermannControlCommand current_ackermann_cmd_{}; + AckermannControlCommand current_manual_ackermann_cmd_{}; + GearCommand current_gear_cmd_{}; + GearCommand current_manual_gear_cmd_{}; + TurnIndicatorsCommand::ConstSharedPtr current_turn_indicators_cmd_ptr_{}; + HazardLightsCommand::ConstSharedPtr current_hazard_lights_cmd_ptr_{}; + Trajectory::ConstSharedPtr current_trajectory_ptr_{}; + bool simulate_motion_ = true; //!< stop vehicle motion simulation if false + ControlModeReport current_control_mode_{}; + bool enable_road_slope_simulation_ = true; /* frame_id */ - std::string simulated_frame_id_; //!< @brief simulated vehicle frame id - std::string origin_frame_id_; //!< @brief map frame_id + std::string simulated_frame_id_ = ""; //!< @brief simulated vehicle frame id + std::string origin_frame_id_ = ""; //!< @brief map frame_id /* flags */ - bool is_initialized_; //!< @brief flag to check the initial position is set - bool add_measurement_noise_; //!< @brief flag to add measurement noise + bool is_initialized_ = false; //!< @brief flag to check the initial position is set + bool add_measurement_noise_ = false; //!< @brief flag to add measurement noise - DeltaTime delta_time_; //!< @brief to calculate delta time + DeltaTime delta_time_{}; //!< @brief to calculate delta time - MeasurementNoiseGenerator measurement_noise_; //!< @brief for measurement noise + MeasurementNoiseGenerator measurement_noise_{}; //!< @brief for measurement noise - double x_stddev_; //!< @brief x standard deviation for dummy covariance in map coordinate - double y_stddev_; //!< @brief y standard deviation for dummy covariance in map coordinate + double x_stddev_ = 0.0; //!< @brief x standard deviation for dummy covariance in map coordinate + double y_stddev_ = 0.0; //!< @brief y standard deviation for dummy covariance in map coordinate /* vehicle model */ enum class VehicleModelType { diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp index 9e7c101e76407..289c607a18d90 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp @@ -39,11 +39,12 @@ class SimModelDelaySteerAcc : public SimModelInterface * @param [in] acc_time_constant time constant for 1D model of accel dynamics * @param [in] steer_delay time delay for steering command [s] * @param [in] steer_time_constant time constant for 1D model of steering dynamics + * @param [in] steer_dead_band dead band for steering angle [rad] */ SimModelDelaySteerAcc( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant); + double steer_time_constant, double steer_dead_band); /** * @brief default destructor @@ -79,6 +80,7 @@ class SimModelDelaySteerAcc : public SimModelInterface const double acc_time_constant_; //!< @brief time constant for accel dynamics const double steer_delay_; //!< @brief time delay for steering command [s] const double steer_time_constant_; //!< @brief time constant for steering dynamics + const double steer_dead_band_; //!< @brief dead band for steering angle [rad] /** * @brief set queue buffer for input command diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp index 1beec46a48ee9..3bda878f8cd76 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp @@ -39,11 +39,12 @@ class SimModelDelaySteerAccGeared : public SimModelInterface * @param [in] acc_time_constant time constant for 1D model of accel dynamics * @param [in] steer_delay time delay for steering command [s] * @param [in] steer_time_constant time constant for 1D model of steering dynamics + * @param [in] steer_dead_band dead band for steering angle [rad] */ SimModelDelaySteerAccGeared( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant); + double steer_time_constant, double steer_dead_band); /** * @brief default destructor @@ -79,6 +80,7 @@ class SimModelDelaySteerAccGeared : public SimModelInterface const double acc_time_constant_; //!< @brief time constant for accel dynamics const double steer_delay_; //!< @brief time delay for steering command [s] const double steer_time_constant_; //!< @brief time constant for steering dynamics + const double steer_dead_band_; //!< @brief dead band for steering angle [rad] /** * @brief set queue buffer for input command diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp index 30cf5d3c7c24a..5c1ec55d522bf 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp @@ -42,11 +42,12 @@ class SimModelDelaySteerVel : public SimModelInterface * @param [in] vx_time_constant time constant for 1D model of velocity dynamics * @param [in] steer_delay time delay for steering command [s] * @param [in] steer_time_constant time constant for 1D model of steering dynamics + * @param [in] steer_dead_band dead band for steering angle [rad] */ SimModelDelaySteerVel( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double vx_delay, double vx_time_constant, double steer_delay, - double steer_time_constant); + double steer_time_constant, double steer_dead_band); /** * @brief destructor @@ -84,6 +85,7 @@ class SimModelDelaySteerVel : public SimModelInterface const double steer_delay_; //!< @brief time delay for angular-velocity command [s] const double steer_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics + const double steer_dead_band_; //!< @brief dead band for steering angle [rad] /** * @brief set queue buffer for input command diff --git a/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml b/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml index d7c002a1fcaa9..dbd56bf9e9bff 100644 --- a/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml +++ b/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml @@ -14,6 +14,7 @@ acc_time_constant: 0.1 steer_time_delay: 0.1 steer_time_constant: 0.1 + steer_dead_band: 0.0 x_stddev: 0.0001 # x standard deviation for dummy covariance in map coordinate y_stddev: 0.0001 # y standard deviation for dummy covariance in map coordinate diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index fb6a5457e8f05..d7e25860c8abf 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -229,6 +229,7 @@ void SimplePlanningSimulator::initialize_vehicle_model() const double vel_time_constant = declare_parameter("vel_time_constant", 0.5); const double steer_time_delay = declare_parameter("steer_time_delay", 0.24); const double steer_time_constant = declare_parameter("steer_time_constant", 0.27); + const double steer_dead_band = declare_parameter("steer_dead_band", 0.0); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); const double wheelbase = vehicle_info.wheel_base_m; @@ -245,17 +246,17 @@ void SimplePlanningSimulator::initialize_vehicle_model() vehicle_model_type_ = VehicleModelType::DELAY_STEER_VEL; vehicle_model_ptr_ = std::make_shared( vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, - vel_time_delay, vel_time_constant, steer_time_delay, steer_time_constant); + vel_time_delay, vel_time_constant, steer_time_delay, steer_time_constant, steer_dead_band); } else if (vehicle_model_type_str == "DELAY_STEER_ACC") { vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC; vehicle_model_ptr_ = std::make_shared( vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, - acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant); + acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band); } else if (vehicle_model_type_str == "DELAY_STEER_ACC_GEARED") { vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC_GEARED; vehicle_model_ptr_ = std::make_shared( vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, - acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant); + acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band); } else { throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp index 870444acb5215..964157cdeb64c 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp @@ -19,7 +19,7 @@ SimModelDelaySteerAcc::SimModelDelaySteerAcc( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant) + double steer_time_constant, double steer_dead_band) : SimModelInterface(6 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -30,7 +30,8 @@ SimModelDelaySteerAcc::SimModelDelaySteerAcc( acc_delay_(acc_delay), acc_time_constant_(std::max(acc_time_constant, MIN_TIME_CONSTANT)), steer_delay_(steer_delay), - steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)) + steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), + steer_dead_band_(steer_dead_band) { initializeInputQueue(dt); } @@ -105,8 +106,18 @@ Eigen::VectorXd SimModelDelaySteerAcc::calcModel( const double steer = state(IDX::STEER); const double acc_des = sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_); const double steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_); - double steer_rate = -(steer - steer_des) / steer_time_constant_; - steer_rate = sat(steer_rate, steer_rate_lim_, -steer_rate_lim_); + const double steer_diff = steer - steer_des; + const double steer_diff_with_dead_band = std::invoke([&]() { + if (steer_diff > steer_dead_band_) { + return steer_diff - steer_dead_band_; + } else if (steer_diff < -steer_dead_band_) { + return steer_diff + steer_dead_band_; + } else { + return 0.0; + } + }); + const double steer_rate = + sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_); Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vel * cos(yaw); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index 5170ed914e0f1..76669c9490fc6 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -21,7 +21,7 @@ SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant) + double steer_time_constant, double steer_dead_band) : SimModelInterface(6 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -32,7 +32,9 @@ SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( acc_delay_(acc_delay), acc_time_constant_(std::max(acc_time_constant, MIN_TIME_CONSTANT)), steer_delay_(steer_delay), - steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)) + steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), + steer_dead_band_(steer_dead_band) + { initializeInputQueue(dt); } @@ -113,8 +115,18 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel( const double steer = state(IDX::STEER); const double acc_des = sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_); const double steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_); - double steer_rate = -(steer - steer_des) / steer_time_constant_; - steer_rate = sat(steer_rate, steer_rate_lim_, -steer_rate_lim_); + const double steer_diff = steer - steer_des; + const double steer_diff_with_dead_band = std::invoke([&]() { + if (steer_diff > steer_dead_band_) { + return steer_diff - steer_dead_band_; + } else if (steer_diff < -steer_dead_band_) { + return steer_diff + steer_dead_band_; + } else { + return 0.0; + } + }); + const double steer_rate = + sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_); Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vel * cos(yaw); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp index 1ac71fbb58c23..6d155ee921107 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp @@ -19,7 +19,7 @@ SimModelDelaySteerVel::SimModelDelaySteerVel( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double vx_delay, double vx_time_constant, double steer_delay, - double steer_time_constant) + double steer_time_constant, double steer_dead_band) : SimModelInterface(5 /* dim x */, 2 /* dim u */), MIN_TIME_CONSTANT(0.03), vx_lim_(vx_lim), @@ -30,7 +30,8 @@ SimModelDelaySteerVel::SimModelDelaySteerVel( vx_delay_(vx_delay), vx_time_constant_(std::max(vx_time_constant, MIN_TIME_CONSTANT)), steer_delay_(steer_delay), - steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)) + steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), + steer_dead_band_(steer_dead_band) { initializeInputQueue(dt); } @@ -106,11 +107,20 @@ Eigen::VectorXd SimModelDelaySteerVel::calcModel( const double delay_input_vx = input(IDX_U::VX_DES); const double delay_input_steer = input(IDX_U::STEER_DES); const double delay_vx_des = sat(delay_input_vx, vx_lim_, -vx_lim_); + const double vx_rate = sat(-(vx - delay_vx_des) / vx_time_constant_, vx_rate_lim_, -vx_rate_lim_); const double delay_steer_des = sat(delay_input_steer, steer_lim_, -steer_lim_); - double vx_rate = -(vx - delay_vx_des) / vx_time_constant_; - double steer_rate = -(steer - delay_steer_des) / steer_time_constant_; - vx_rate = sat(vx_rate, vx_rate_lim_, -vx_rate_lim_); - steer_rate = sat(steer_rate, steer_rate_lim_, -steer_rate_lim_); + const double steer_diff = steer - delay_steer_des; + const double steer_diff_with_dead_band = std::invoke([&]() { + if (steer_diff > steer_dead_band_) { + return steer_diff - steer_dead_band_; + } else if (steer_diff < -steer_dead_band_) { + return steer_diff + steer_dead_band_; + } else { + return 0.0; + } + }); + const double steer_rate = + sat(-steer_diff_with_dead_band / steer_time_constant_, steer_rate_lim_, -steer_rate_lim_); Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); d_state(IDX::X) = vx * cos(yaw); diff --git a/system/system_diagnostic_graph/CMakeLists.txt b/system/system_diagnostic_graph/CMakeLists.txt index 81d8ba39b3d1b..142aa94eeef69 100644 --- a/system/system_diagnostic_graph/CMakeLists.txt +++ b/system/system_diagnostic_graph/CMakeLists.txt @@ -4,13 +4,15 @@ project(system_diagnostic_graph) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(aggregator +ament_auto_add_library(${PROJECT_NAME} SHARED src/core/config.cpp src/core/debug.cpp src/core/graph.cpp - src/core/nodes.cpp - src/core/exprs.cpp + src/core/units.cpp src/core/modes.cpp +) + +ament_auto_add_executable(aggregator src/main.cpp ) @@ -18,9 +20,11 @@ ament_auto_add_executable(converter src/tool.cpp ) -install( - PROGRAMS util/debug.py - DESTINATION lib/${PROJECT_NAME} -) +if(BUILD_TESTING) + get_filename_component(RESOURCE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/test/files ABSOLUTE) + ament_auto_add_gtest(gtest_${PROJECT_NAME} test/src/test.cpp) + target_compile_definitions(gtest_${PROJECT_NAME} PRIVATE TEST_RESOURCE_PATH="${RESOURCE_PATH}") + target_include_directories(gtest_${PROJECT_NAME} PRIVATE src) +endif() ament_auto_package(INSTALL_TO_SHARE config example launch) diff --git a/system/system_diagnostic_graph/example/example_0.yaml b/system/system_diagnostic_graph/example/example_0.yaml index 0ee6e59c8e121..fc4436bba1595 100644 --- a/system/system_diagnostic_graph/example/example_0.yaml +++ b/system/system_diagnostic_graph/example/example_0.yaml @@ -1,32 +1,37 @@ files: - - { package: system_diagnostic_graph, path: example/example_1.yaml } - - { package: system_diagnostic_graph, path: example/example_2.yaml } + - { path: $(find-pkg-share system_diagnostic_graph)/example/example_1.yaml } + - { path: $(find-pkg-share system_diagnostic_graph)/example/example_2.yaml } nodes: - path: /autoware/modes/stop - type: debug-ok + type: ok - path: /autoware/modes/autonomous type: and list: - - { type: link, path: /functions/pose_estimation } - - { type: link, path: /functions/obstacle_detection } + - { type: link, link: /functions/pose_estimation } + - { type: link, link: /functions/obstacle_detection } - path: /autoware/modes/local type: and list: - - { type: link, path: /external/joystick_command } + - { type: link, link: /external/joystick_command } - path: /autoware/modes/remote type: and list: - - { type: link, path: /external/remote_command } + - { type: link, link: /external/remote_command } - path: /autoware/modes/emergency-stop - type: debug-ok + type: ok - path: /autoware/modes/comfortable-stop - type: debug-ok + type: and + list: + - { type: link, link: /functions/obstacle_detection } - path: /autoware/modes/pull-over - type: debug-ok + type: and + list: + - { type: link, link: /functions/pose_estimation } + - { type: link, link: /functions/obstacle_detection } diff --git a/system/system_diagnostic_graph/example/example_1.yaml b/system/system_diagnostic_graph/example/example_1.yaml index 5ba93ec3059e4..07d4951b89446 100644 --- a/system/system_diagnostic_graph/example/example_1.yaml +++ b/system/system_diagnostic_graph/example/example_1.yaml @@ -2,22 +2,22 @@ nodes: - path: /functions/pose_estimation type: and list: - - { type: link, path: /sensing/lidars/top } + - { type: link, link: /sensing/lidars/top } - path: /functions/obstacle_detection type: or list: - - { type: link, path: /sensing/lidars/front } - - { type: link, path: /sensing/radars/front } + - { type: link, link: /sensing/lidars/front } + - { type: link, link: /sensing/radars/front } - path: /sensing/lidars/top type: diag - name: "lidar_driver/top: status" + diag: "lidar_driver/top: status" - path: /sensing/lidars/front type: diag - name: "lidar_driver/front: status" + diag: "lidar_driver/front: status" - path: /sensing/radars/front type: diag - name: "radar_driver/front: status" + diag: "radar_driver/front: status" diff --git a/system/system_diagnostic_graph/example/example_2.yaml b/system/system_diagnostic_graph/example/example_2.yaml index f61f4accbfec8..a03701b661ba9 100644 --- a/system/system_diagnostic_graph/example/example_2.yaml +++ b/system/system_diagnostic_graph/example/example_2.yaml @@ -1,8 +1,8 @@ nodes: - path: /external/joystick_command type: diag - name: "external_command_checker: joystick_command" + diag: "external_command_checker: joystick_command" - path: /external/remote_command type: diag - name: "external_command_checker: remote_command" + diag: "external_command_checker: remote_command" diff --git a/system/system_diagnostic_graph/package.xml b/system/system_diagnostic_graph/package.xml index ffc0c6c7d5385..7855edcde9c62 100644 --- a/system/system_diagnostic_graph/package.xml +++ b/system/system_diagnostic_graph/package.xml @@ -15,6 +15,7 @@ tier4_system_msgs yaml_cpp_vendor + ament_cmake_gtest ament_lint_auto autoware_lint_common diff --git a/system/system_diagnostic_graph/src/core/config.cpp b/system/system_diagnostic_graph/src/core/config.cpp index 305860af32840..2339e96f3951f 100644 --- a/system/system_diagnostic_graph/src/core/config.cpp +++ b/system/system_diagnostic_graph/src/core/config.cpp @@ -14,11 +14,15 @@ #include "config.hpp" +#include "error.hpp" + #include #include #include +#include #include +#include #include // DEBUG @@ -27,235 +31,265 @@ namespace system_diagnostic_graph { -ErrorMarker::ErrorMarker(const std::string & file) +template +void extend(std::vector & u, const std::vector & v) { - file_ = file; + u.insert(u.end(), v.begin(), v.end()); } -std::string ErrorMarker::str() const +template +auto enumerate(const std::vector & v) { - if (type_.empty()) { - return file_; - } - - std::string result = type_; - for (const auto & index : indices_) { - result += "-" + std::to_string(index); + std::vector> result; + for (size_t i = 0; i < v.size(); ++i) { + result.push_back(std::make_pair(i, v[i])); } - return result + " in " + file_; -} - -ErrorMarker ErrorMarker::type(const std::string & type) const -{ - ErrorMarker mark = *this; - mark.type_ = type; - return mark; + return result; } -ErrorMarker ErrorMarker::index(size_t index) const +ConfigData::ConfigData(const std::string & path) { - ErrorMarker mark = *this; - mark.indices_.push_back(index); - return mark; + file = path; } -ConfigObject::ConfigObject(const ErrorMarker & mark, YAML::Node yaml, const std::string & type) +ConfigData ConfigData::load(YAML::Node yaml) { if (!yaml.IsMap()) { - throw create_error(mark, type + " is not a dict type"); + throw error("object is not a dict type", *this); } for (const auto & kv : yaml) { - dict_[kv.first.as()] = kv.second; + object[kv.first.as()] = kv.second; } - mark_ = mark; - type_ = type; + return *this; } -ErrorMarker ConfigObject::mark() const +ConfigData ConfigData::type(const std::string & name) const { - return mark_; + ConfigData data(file); + data.mark = name; + return data; } -std::optional ConfigObject::take_yaml(const std::string & name) +ConfigData ConfigData::node(const size_t index) const { - if (!dict_.count(name)) { - return std::nullopt; - } - const auto yaml = dict_.at(name); - dict_.erase(name); - return yaml; + ConfigData data(file); + data.mark = mark + "-" + std::to_string(index); + return data; } -std::string ConfigObject::take_text(const std::string & name) +std::string ConfigData::take_text(const std::string & name) { - if (!dict_.count(name)) { - throw create_error(mark_, "object has no '" + name + "' field"); + if (!object.count(name)) { + throw error("required field is not found", name, *this); } - const auto yaml = dict_.at(name); - dict_.erase(name); + const auto yaml = object.at(name); + object.erase(name); return yaml.as(); } -std::string ConfigObject::take_text(const std::string & name, const std::string & fail) +std::string ConfigData::take_text(const std::string & name, const std::string & fail) { - if (!dict_.count(name)) { + if (!object.count(name)) { return fail; } - const auto yaml = dict_.at(name); - dict_.erase(name); + const auto yaml = object.at(name); + object.erase(name); return yaml.as(); } -std::vector ConfigObject::take_list(const std::string & name) +std::vector ConfigData::take_list(const std::string & name) { - if (!dict_.count(name)) { + if (!object.count(name)) { return std::vector(); } - const auto yaml = dict_.at(name); - dict_.erase(name); + const auto yaml = object.at(name); + object.erase(name); if (!yaml.IsSequence()) { - throw ConfigError("the '" + name + "' field is not a list type"); + throw error("field is not a list type", name, *this); } return std::vector(yaml.begin(), yaml.end()); } -bool ConfigFilter::check(const std::string & mode) const +void check_config_nodes(const std::vector & nodes) { - if (!excludes.empty() && excludes.count(mode) != 0) return false; - if (!includes.empty() && includes.count(mode) == 0) return false; - return true; -} + std::unordered_map path_count; + for (const auto & node : nodes) { + path_count[node->path] += 1; + } -ConfigError create_error(const ErrorMarker & mark, const std::string & message) -{ - (void)mark; - return ConfigError(message); + path_count.erase(""); + for (const auto & [path, count] : path_count) { + if (1 < count) { + throw error("object path is not unique", path); + } + } } -ConfigFilter parse_mode_filter(const ErrorMarker & mark, std::optional yaml) +void resolve_link_nodes(std::vector & nodes) { - std::unordered_set excludes; - std::unordered_set includes; - if (yaml) { - ConfigObject dict(mark, yaml.value(), "mode filter"); + std::vector filtered; + std::unordered_map links; + std::unordered_map paths; - for (const auto & mode : dict.take_list("except")) { - excludes.emplace(mode.as()); + for (const auto & node : nodes) { + links[node] = node; + paths[node->path] = node; + } + + for (const auto & node : nodes) { + if (node->type == "link" && node->path == "") { + const auto link = node->data.take_text("link"); + if (!paths.count(link)) { + throw error("link path is not found", link, node->data); + } + links[node] = paths.at(link); + } else { + filtered.push_back(node); } - for (const auto & mode : dict.take_list("only")) { - includes.emplace(mode.as()); + } + nodes = filtered; + + for (const auto & node : nodes) { + for (auto & child : node->children) { + child = links.at(child); } } - return ConfigFilter{excludes, includes}; } -FileConfig parse_file_config(const ErrorMarker & mark, YAML::Node yaml) +std::string complement_node_type(ConfigData & data) { - ConfigObject dict(mark, yaml, "file object"); - const auto relative_path = dict.take_text("path"); - const auto package_name = dict.take_text("package"); - const auto package_path = ament_index_cpp::get_package_share_directory(package_name); - return FileConfig{mark, package_path + "/" + relative_path}; + if (data.object.count("diag")) { + return "diag"; + } + return data.take_text("type"); } -NodeConfig parse_node_config(const ErrorMarker & mark, YAML::Node yaml) +std::string resolve_substitution(const std::string & substitution, const ConfigData & data) { - ConfigObject dict(mark, yaml, "node object"); - const auto path = dict.take_text("path"); - const auto mode = parse_mode_filter(dict.mark(), dict.take_yaml("mode")); - return NodeConfig{path, mode, dict}; + std::stringstream ss(substitution); + std::string word; + std::vector words; + while (getline(ss, word, ' ')) { + words.push_back(word); + } + + if (words.size() == 2 && words[0] == "find-pkg-share") { + return ament_index_cpp::get_package_share_directory(words[1]); + } + if (words.size() == 1 && words[0] == "dirname") { + return std::filesystem::path(data.file).parent_path(); + } + throw error("unknown substitution", substitution, data); } -ExprConfig parse_expr_config(const ErrorMarker & mark, YAML::Node yaml) +std::string resolve_file_path(const std::string & path, const ConfigData & data) { - ConfigObject dict(mark, yaml, "expr object"); - return parse_expr_config(dict); + static const std::regex pattern(R"(\$\(([^()]*)\))"); + std::smatch m; + std::string result = path; + while (std::regex_search(result, m, pattern)) { + const std::string prefix = m.prefix(); + const std::string suffix = m.suffix(); + result = prefix + resolve_substitution(m.str(1), data) + suffix; + } + return result; } -ExprConfig parse_expr_config(ConfigObject & dict) +PathConfig::SharedPtr parse_path_config(const ConfigData & data) { - const auto type = dict.take_text("type"); - const auto mode = parse_mode_filter(dict.mark(), dict.take_yaml("mode")); - return ExprConfig{type, mode, dict}; + const auto path = std::make_shared(data); + path->original = path->data.take_text("path"); + path->resolved = resolve_file_path(path->original, path->data); + return path; } -void dump(const ConfigFile & config) +UnitConfig::SharedPtr parse_node_config(const ConfigData & data) { - std::cout << "=================================================================" << std::endl; - std::cout << config.mark.str() << std::endl; - for (const auto & file : config.files) { - std::cout << " - f: " << file.path << " (" << file.mark.str() << ")" << std::endl; - } - for (const auto & unit : config.units) { - std::cout << " - u: " << unit.path << " (" << unit.dict.mark().str() << ")" << std::endl; + const auto node = std::make_shared(data); + node->path = node->data.take_text("path", ""); + node->type = node->data.take_text("type", ""); + + if (node->type.empty()) { + node->type = complement_node_type(node->data); } - for (const auto & diag : config.diags) { - std::cout << " - d: " << diag.path << " (" << diag.dict.mark().str() << ")" << std::endl; + + for (const auto & [index, yaml] : enumerate(node->data.take_list("list"))) { + const auto child = data.node(index).load(yaml); + node->children.push_back(parse_node_config(child)); } + return node; } -template -auto apply(const ErrorMarker & mark, F & func, const std::vector & list) +FileConfig::SharedPtr parse_file_config(const ConfigData & data) { - std::vector result; - for (size_t i = 0; i < list.size(); ++i) { - result.push_back(func(mark.index(i), list[i])); + const auto file = std::make_shared(data); + const auto path_data = data.type("file"); + const auto node_data = data.type("node"); + const auto paths = file->data.take_list("files"); + const auto nodes = file->data.take_list("nodes"); + + for (const auto & [index, yaml] : enumerate(paths)) { + const auto path = path_data.node(index).load(yaml); + file->paths.push_back(parse_path_config(path)); } - return result; + for (const auto & [index, yaml] : enumerate(nodes)) { + const auto node = node_data.node(index).load(yaml); + file->nodes.push_back(parse_node_config(node)); + } + return file; } -ConfigFile load_config_file(const FileConfig & file) +FileConfig::SharedPtr load_file_config(PathConfig & config) { - if (!std::filesystem::exists(file.path)) { - throw create_error(file.mark, "config file '" + file.path + "' does not exist"); + const auto path = std::filesystem::path(config.resolved); + if (!std::filesystem::exists(path)) { + throw error("file is not found", path, config.data); } + const auto file = ConfigData(path).load(YAML::LoadFile(path)); + return parse_file_config(file); +} - const auto yaml = YAML::LoadFile(file.path); - const auto mark = ErrorMarker(file.path); - auto dict = ConfigObject(mark, yaml, "config file"); +RootConfig load_root_config(const PathConfig::SharedPtr root) +{ + std::vector paths; + paths.push_back(root); + + std::vector files; + for (size_t i = 0; i < paths.size(); ++i) { + const auto path = paths[i]; + const auto file = load_file_config(*path); + files.push_back(file); + extend(paths, file->paths); + } - std::vector units; - std::vector diags; - for (const auto & node : dict.take_list("nodes")) { - const auto type = node["type"].as(); - if (type == "diag") { - diags.push_back(node); - } else { - units.push_back(node); - } + std::vector nodes; + for (const auto & file : files) { + extend(nodes, file->nodes); } + for (size_t i = 0; i < nodes.size(); ++i) { + const auto node = nodes[i]; + extend(nodes, node->children); + } + + check_config_nodes(nodes); + resolve_link_nodes(nodes); - ConfigFile config(mark); - config.files = apply(mark.type("file"), parse_file_config, dict.take_list("files")); - config.units = apply(mark.type("unit"), parse_node_config, units); - config.diags = apply(mark.type("diag"), parse_node_config, diags); + RootConfig config; + config.files = files; + config.nodes = nodes; return config; } -ConfigFile load_config_root(const std::string & path) +RootConfig load_root_config(const std::string & path) { - const auto mark = ErrorMarker("root file"); - std::vector configs; - configs.push_back(load_config_file(FileConfig{mark, path})); - - // Use an index because updating the vector invalidates the iterator. - for (size_t i = 0; i < configs.size(); ++i) { - for (const auto & file : configs[i].files) { - configs.push_back(load_config_file(file)); - } - } - - ConfigFile result(mark); - for (const auto & config : configs) { - result.files.insert(result.files.end(), config.files.begin(), config.files.end()); - result.units.insert(result.units.end(), config.units.begin(), config.units.end()); - result.diags.insert(result.diags.end(), config.diags.begin(), config.diags.end()); - } - return result; + const auto root = std::make_shared(ConfigData("root-file")); + root->original = path; + root->resolved = path; + return load_root_config(root); } } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/config.hpp b/system/system_diagnostic_graph/src/core/config.hpp index 4d679ef575944..d959f5b6be8aa 100644 --- a/system/system_diagnostic_graph/src/core/config.hpp +++ b/system/system_diagnostic_graph/src/core/config.hpp @@ -18,94 +18,92 @@ #include #include -#include -#include #include #include -#include #include namespace system_diagnostic_graph { -struct ConfigError : public std::runtime_error +struct ConfigData { - using runtime_error::runtime_error; -}; - -class ErrorMarker -{ -public: - explicit ErrorMarker(const std::string & file = ""); - std::string str() const; - ErrorMarker type(const std::string & type) const; - ErrorMarker index(size_t index) const; - -private: - std::string file_; - std::string type_; - std::vector indices_; -}; + explicit ConfigData(const std::string & file); + ConfigData load(YAML::Node yaml); + ConfigData type(const std::string & name) const; + ConfigData node(const size_t index) const; -class ConfigObject -{ -public: - ConfigObject(const ErrorMarker & mark, YAML::Node yaml, const std::string & type); - ErrorMarker mark() const; - std::optional take_yaml(const std::string & name); std::string take_text(const std::string & name); std::string take_text(const std::string & name, const std::string & fail); std::vector take_list(const std::string & name); -private: - ErrorMarker mark_; - std::string type_; - std::unordered_map dict_; + std::string file; + std::string mark; + std::unordered_map object; }; -struct ConfigFilter +struct BaseConfig { - bool check(const std::string & mode) const; - std::unordered_set excludes; - std::unordered_set includes; + explicit BaseConfig(const ConfigData & data) : data(data) {} + ConfigData data; }; -struct ExprConfig +struct PathConfig : public BaseConfig { - std::string type; - ConfigFilter mode; - ConfigObject dict; + using SharedPtr = std::shared_ptr; + using BaseConfig::BaseConfig; + std::string original; + std::string resolved; }; -struct NodeConfig +struct UnitConfig : public BaseConfig { + using SharedPtr = std::shared_ptr; + using BaseConfig::BaseConfig; + std::string type; std::string path; - ConfigFilter mode; - ConfigObject dict; + std::vector children; }; -struct FileConfig +struct FileConfig : public BaseConfig { - ErrorMarker mark; - std::string path; + using SharedPtr = std::shared_ptr; + using BaseConfig::BaseConfig; + std::vector paths; + std::vector nodes; }; -struct ConfigFile +struct RootConfig { - explicit ConfigFile(const ErrorMarker & mark) : mark(mark) {} - ErrorMarker mark; - std::vector files; - std::vector units; - std::vector diags; + std::vector files; + std::vector nodes; }; -using ConfigDict = std::unordered_map; +template +T error(const std::string & text, const ConfigData & data) +{ + const auto hint = data.mark.empty() ? data.file : data.mark + ":" + data.file; + return T(text + " (" + hint + ")"); +} + +template +T error(const std::string & text) +{ + return T(text); +} -ConfigError create_error(const ErrorMarker & mark, const std::string & message); -ConfigFile load_config_root(const std::string & path); +template +T error(const std::string & text, const std::string & value, const ConfigData & data) +{ + return error(text + ": " + value, data); +} + +template +T error(const std::string & text, const std::string & value) +{ + return error(text + ": " + value); +} -ExprConfig parse_expr_config(const ErrorMarker & mark, YAML::Node yaml); -ExprConfig parse_expr_config(ConfigObject & dict); +RootConfig load_root_config(const std::string & path); } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/debug.cpp b/system/system_diagnostic_graph/src/core/debug.cpp index ed696573521a7..f14177f4571ad 100644 --- a/system/system_diagnostic_graph/src/core/debug.cpp +++ b/system/system_diagnostic_graph/src/core/debug.cpp @@ -15,8 +15,8 @@ #include "debug.hpp" #include "graph.hpp" -#include "nodes.hpp" #include "types.hpp" +#include "units.hpp" #include #include @@ -36,7 +36,9 @@ void Graph::debug() { std::vector lines; for (const auto & node : nodes_) { - lines.push_back(node->debug()); + const auto level_name = level_names.at(node->level()); + const auto index_name = std::to_string(node->index()); + lines.push_back({"unit", index_name, level_name, node->path(), "-----"}); } std::array widths = {}; @@ -57,25 +59,4 @@ void Graph::debug() } } -DiagDebugData UnitNode::debug() const -{ - const auto level_name = level_names.at(level()); - const auto index_name = std::to_string(index()); - return {"unit", index_name, level_name, path_, "-----"}; -} - -DiagDebugData DiagNode::debug() const -{ - const auto level_name = level_names.at(level()); - const auto index_name = std::to_string(index()); - return {"diag", index_name, level_name, path_, name_}; -} - -DiagDebugData UnknownNode::debug() const -{ - const auto level_name = level_names.at(level()); - const auto index_name = std::to_string(index()); - return {"test", index_name, level_name, path_, "-----"}; -} - } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/error.hpp b/system/system_diagnostic_graph/src/core/error.hpp new file mode 100644 index 0000000000000..2c10d659f2df4 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/error.hpp @@ -0,0 +1,65 @@ +// 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 CORE__ERROR_HPP_ +#define CORE__ERROR_HPP_ + +#include + +namespace system_diagnostic_graph +{ + +struct Exception : public std::runtime_error +{ + using runtime_error::runtime_error; +}; + +class FileNotFound : public Exception +{ + using Exception::Exception; +}; + +class UnknownType : public Exception +{ + using Exception::Exception; +}; + +class InvalidType : public Exception +{ + using Exception::Exception; +}; + +class FieldNotFound : public Exception +{ + using Exception::Exception; +}; + +class PathConflict : public Exception +{ + using Exception::Exception; +}; + +class PathNotFound : public Exception +{ + using Exception::Exception; +}; + +class GraphStructure : public Exception +{ + using Exception::Exception; +}; + +} // namespace system_diagnostic_graph + +#endif // CORE__ERROR_HPP_ diff --git a/system/system_diagnostic_graph/src/core/exprs.cpp b/system/system_diagnostic_graph/src/core/exprs.cpp deleted file mode 100644 index 22281f939cad2..0000000000000 --- a/system/system_diagnostic_graph/src/core/exprs.cpp +++ /dev/null @@ -1,216 +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 "exprs.hpp" - -#include "nodes.hpp" - -#include -#include -#include -#include -#include - -// DEBUG -#include - -namespace system_diagnostic_graph -{ - -using LinkStatus = std::vector>; - -void extend(LinkStatus & a, const LinkStatus & b) -{ - a.insert(a.end(), b.begin(), b.end()); -} - -void extend_false(LinkStatus & a, const LinkStatus & b) -{ - for (const auto & p : b) { - a.push_back(std::make_pair(p.first, false)); - } -} - -auto create_expr_list(ExprInit & exprs, ConfigObject & config) -{ - std::vector> result; - const auto list = config.take_list("list"); - for (size_t i = 0; i < list.size(); ++i) { - auto dict = parse_expr_config(config.mark().index(i), list[i]); - auto expr = exprs.create(dict); - if (expr) { - result.push_back(std::move(expr)); - } - } - return result; -} - -ConstExpr::ConstExpr(const DiagnosticLevel level) -{ - level_ = level; -} - -ExprStatus ConstExpr::eval() const -{ - ExprStatus status; - status.level = level_; - return status; -} - -std::vector ConstExpr::get_dependency() const -{ - return {}; -} - -LinkExpr::LinkExpr(ExprInit & exprs, ConfigObject & config) -{ - // TODO(Takagi, Isamu): remove - (void)config; - (void)exprs; -} - -void LinkExpr::init(ConfigObject & config, std::unordered_map nodes) -{ - const auto path = config.take_text("path"); - if (!nodes.count(path)) { - throw ConfigError("node path '" + path + "' does not exist"); - } - node_ = nodes.at(path); -} - -ExprStatus LinkExpr::eval() const -{ - ExprStatus status; - status.level = node_->level(); - status.links.push_back(std::make_pair(node_, true)); - return status; -} - -std::vector LinkExpr::get_dependency() const -{ - return {node_}; -} - -AndExpr::AndExpr(ExprInit & exprs, ConfigObject & config, bool short_circuit) -{ - list_ = create_expr_list(exprs, config); - short_circuit_ = short_circuit; -} - -ExprStatus AndExpr::eval() const -{ - if (list_.empty()) { - ExprStatus status; - status.level = DiagnosticStatus::OK; - return status; - } - - ExprStatus status; - status.level = DiagnosticStatus::OK; - for (const auto & expr : list_) { - const auto result = expr->eval(); - status.level = std::max(status.level, result.level); - extend(status.links, result.links); - if (short_circuit_ && status.level != DiagnosticStatus::OK) { - break; - } - } - status.level = std::min(status.level, DiagnosticStatus::ERROR); - return status; -} - -std::vector AndExpr::get_dependency() const -{ - std::vector depends; - for (const auto & expr : list_) { - const auto nodes = expr->get_dependency(); - depends.insert(depends.end(), nodes.begin(), nodes.end()); - } - return depends; -} - -OrExpr::OrExpr(ExprInit & exprs, ConfigObject & config) -{ - list_ = create_expr_list(exprs, config); -} - -ExprStatus OrExpr::eval() const -{ - if (list_.empty()) { - ExprStatus status; - status.level = DiagnosticStatus::OK; - return status; - } - - ExprStatus status; - status.level = DiagnosticStatus::ERROR; - for (const auto & expr : list_) { - const auto result = expr->eval(); - status.level = std::min(status.level, result.level); - extend(status.links, result.links); - } - status.level = std::min(status.level, DiagnosticStatus::ERROR); - return status; -} - -std::vector OrExpr::get_dependency() const -{ - std::vector depends; - for (const auto & expr : list_) { - const auto nodes = expr->get_dependency(); - depends.insert(depends.end(), nodes.begin(), nodes.end()); - } - return depends; -} - -ExprInit::ExprInit(const std::string & mode) -{ - mode_ = mode; -} - -std::unique_ptr ExprInit::create(ExprConfig config) -{ - if (!config.mode.check(mode_)) { - return nullptr; - } - if (config.type == "link") { - auto expr = std::make_unique(*this, config.dict); - links_.push_back(std::make_pair(expr.get(), config.dict)); - return expr; - } - if (config.type == "and") { - return std::make_unique(*this, config.dict, false); - } - if (config.type == "short-circuit-and") { - return std::make_unique(*this, config.dict, true); - } - if (config.type == "or") { - return std::make_unique(*this, config.dict); - } - if (config.type == "debug-ok") { - return std::make_unique(DiagnosticStatus::OK); - } - if (config.type == "debug-warn") { - return std::make_unique(DiagnosticStatus::WARN); - } - if (config.type == "debug-error") { - return std::make_unique(DiagnosticStatus::ERROR); - } - if (config.type == "debug-stale") { - return std::make_unique(DiagnosticStatus::STALE); - } - throw ConfigError("unknown expr type: " + config.type + " " + config.dict.mark().str()); -} - -} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/exprs.hpp b/system/system_diagnostic_graph/src/core/exprs.hpp deleted file mode 100644 index f582e019d5691..0000000000000 --- a/system/system_diagnostic_graph/src/core/exprs.hpp +++ /dev/null @@ -1,104 +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. - -#ifndef CORE__EXPRS_HPP_ -#define CORE__EXPRS_HPP_ - -#include "config.hpp" -#include "types.hpp" - -#include -#include -#include -#include -#include - -namespace system_diagnostic_graph -{ - -struct ExprStatus -{ - DiagnosticLevel level; - std::vector> links; -}; - -class BaseExpr -{ -public: - virtual ~BaseExpr() = default; - virtual ExprStatus eval() const = 0; - virtual std::vector get_dependency() const = 0; -}; - -class ConstExpr : public BaseExpr -{ -public: - explicit ConstExpr(const DiagnosticLevel level); - ExprStatus eval() const override; - std::vector get_dependency() const override; - -private: - DiagnosticLevel level_; -}; - -class LinkExpr : public BaseExpr -{ -public: - LinkExpr(ExprInit & exprs, ConfigObject & config); - void init(ConfigObject & config, std::unordered_map nodes); - ExprStatus eval() const override; - std::vector get_dependency() const override; - -private: - BaseNode * node_; -}; - -class AndExpr : public BaseExpr -{ -public: - AndExpr(ExprInit & exprs, ConfigObject & config, bool short_circuit); - ExprStatus eval() const override; - std::vector get_dependency() const override; - -private: - bool short_circuit_; - std::vector> list_; -}; - -class OrExpr : public BaseExpr -{ -public: - OrExpr(ExprInit & exprs, ConfigObject & config); - ExprStatus eval() const override; - std::vector get_dependency() const override; - -private: - std::vector> list_; -}; - -class ExprInit -{ -public: - explicit ExprInit(const std::string & mode); - std::unique_ptr create(ExprConfig config); - auto get() const { return links_; } - -private: - std::string mode_; - std::vector> links_; -}; - -} // namespace system_diagnostic_graph - -#endif // CORE__EXPRS_HPP_ diff --git a/system/system_diagnostic_graph/src/core/graph.cpp b/system/system_diagnostic_graph/src/core/graph.cpp index b4fd1d15827f3..96e9bcff5bfd9 100644 --- a/system/system_diagnostic_graph/src/core/graph.cpp +++ b/system/system_diagnostic_graph/src/core/graph.cpp @@ -15,8 +15,8 @@ #include "graph.hpp" #include "config.hpp" -#include "exprs.hpp" -#include "nodes.hpp" +#include "error.hpp" +#include "units.hpp" #include #include @@ -31,12 +31,12 @@ namespace system_diagnostic_graph { -void topological_sort(std::vector> & input) +BaseUnit::UniquePtrList topological_sort(BaseUnit::UniquePtrList && input) { - std::unordered_map degrees; - std::deque nodes; - std::deque result; - std::deque buffer; + std::unordered_map degrees; + std::deque nodes; + std::deque result; + std::deque buffer; // Create a list of raw pointer nodes. for (const auto & node : input) { @@ -45,8 +45,8 @@ void topological_sort(std::vector> & input) // Count degrees of each node. for (const auto & node : nodes) { - for (const auto & link : node->links()) { - ++degrees[link]; + for (const auto & child : node->children()) { + ++degrees[child]; } } @@ -61,9 +61,9 @@ void topological_sort(std::vector> & input) while (!buffer.empty()) { const auto node = buffer.front(); buffer.pop_front(); - for (const auto & link : node->links()) { - if (--degrees[link] == 0) { - buffer.push_back(link); + for (const auto & child : node->children()) { + if (--degrees[child] == 0) { + buffer.push_back(child); } } result.push_back(node); @@ -71,22 +71,51 @@ void topological_sort(std::vector> & input) // Detect circulation because the result does not include the nodes on the loop. if (result.size() != nodes.size()) { - throw ConfigError("detect graph circulation"); + throw error("detect graph circulation"); } // Reverse the result to process from leaf node. - result = std::deque(result.rbegin(), result.rend()); + result = std::deque(result.rbegin(), result.rend()); // Replace node vector. - std::unordered_map indices; + std::unordered_map indices; for (size_t i = 0; i < result.size(); ++i) { indices[result[i]] = i; } - std::vector> sorted(input.size()); + std::vector> sorted(input.size()); for (auto & node : input) { sorted[indices[node.get()]] = std::move(node); } - input = std::move(sorted); + return sorted; +} + +BaseUnit::UniquePtr make_node(const UnitConfig::SharedPtr & config) +{ + if (config->type == "diag") { + return std::make_unique(config->path); + } + if (config->type == "and") { + return std::make_unique(config->path, false); + } + if (config->type == "short-circuit-and") { + return std::make_unique(config->path, true); + } + if (config->type == "or") { + return std::make_unique(config->path); + } + if (config->type == "ok") { + return std::make_unique(config->path, DiagnosticStatus::OK); + } + if (config->type == "warn") { + return std::make_unique(config->path, DiagnosticStatus::WARN); + } + if (config->type == "error") { + return std::make_unique(config->path, DiagnosticStatus::ERROR); + } + if (config->type == "stale") { + return std::make_unique(config->path, DiagnosticStatus::STALE); + } + throw error("unknown node type", config->type, config->data); } Graph::Graph() @@ -101,99 +130,80 @@ Graph::~Graph() void Graph::init(const std::string & file, const std::string & mode) { - ConfigFile root = load_config_root(file); + (void)mode; // TODO(Takagi, Isamu) - std::vector> nodes; - std::unordered_map diags; - std::unordered_map paths; - ExprInit exprs(mode); + BaseUnit::UniquePtrList nodes; + BaseUnit::NodeDict dict; - for (auto & config : root.units) { - if (config.mode.check(mode)) { - auto node = std::make_unique(config.path); - nodes.push_back(std::move(node)); - } - } - for (auto & config : root.diags) { - if (config.mode.check(mode)) { - auto node = std::make_unique(config.path, config.dict); - diags[node->name()] = node.get(); - nodes.push_back(std::move(node)); - } + for (const auto & config : load_root_config(file).nodes) { + const auto node = nodes.emplace_back(make_node(config)).get(); + dict.configs[config] = node; + dict.paths[config->path] = node; } + dict.paths.erase(""); - // TODO(Takagi, Isamu): unknown diag names - { - auto node = std::make_unique("/unknown"); - unknown_ = node.get(); - nodes.push_back(std::move(node)); + for (const auto & [config, node] : dict.configs) { + node->init(config, dict); } - for (const auto & node : nodes) { - paths[node->path()] = node.get(); - } + // Sort units in topological order for update dependencies. + nodes = topological_sort(std::move(nodes)); - for (auto & config : root.units) { - if (paths.count(config.path)) { - paths.at(config.path)->create(config.dict, exprs); - } - } - for (auto & config : root.diags) { - if (paths.count(config.path)) { - paths.at(config.path)->create(config.dict, exprs); - } + for (size_t index = 0; index < nodes.size(); ++index) { + nodes[index]->set_index(index); } - for (auto & [link, config] : exprs.get()) { - link->init(config, paths); - } - - // Sort unit nodes in topological order for update dependencies. - topological_sort(nodes); - - // Set the link index for the ros message. - for (size_t i = 0; i < nodes.size(); ++i) { - nodes[i]->set_index(i); + for (const auto & node : nodes) { + const auto diag = dynamic_cast(node.get()); + if (diag) { + diags_[diag->name()] = diag; + std::cout << diag->name() << std::endl; + } } - nodes_ = std::move(nodes); - diags_ = diags; } -void Graph::callback(const DiagnosticArray & array, const rclcpp::Time & stamp) +void Graph::callback(const rclcpp::Time & stamp, const DiagnosticArray & array) { for (const auto & status : array.status) { const auto iter = diags_.find(status.name); if (iter != diags_.end()) { - iter->second->callback(status, stamp); + iter->second->callback(stamp, status); } else { - unknown_->callback(status, stamp); + // TODO(Takagi, Isamu) + std::cout << "unknown diag: " << status.name << std::endl; } } } -void Graph::update(const rclcpp::Time & stamp) +DiagnosticGraph Graph::report(const rclcpp::Time & stamp) { for (const auto & node : nodes_) { node->update(stamp); } - stamp_ = stamp; -} -DiagnosticGraph Graph::message() const -{ - DiagnosticGraph result; - result.stamp = stamp_; - result.nodes.reserve(nodes_.size()); + DiagnosticGraph message; + message.stamp = stamp; + message.nodes.reserve(nodes_.size()); for (const auto & node : nodes_) { - result.nodes.push_back(node->report()); + const auto report = node->report(); + DiagnosticNode temp; + temp.status.name = node->path(); + temp.status.level = report.level; + for (const auto & [ref, used] : report.links) { + DiagnosticLink link; + link.index = ref->index(); + link.used = used; + temp.links.push_back(link); + } + message.nodes.push_back(temp); } - return result; + return message; } -std::vector Graph::nodes() const +std::vector Graph::nodes() const { - std::vector result; + std::vector result; result.reserve(nodes_.size()); for (const auto & node : nodes_) { result.push_back(node.get()); diff --git a/system/system_diagnostic_graph/src/core/graph.hpp b/system/system_diagnostic_graph/src/core/graph.hpp index e0060c9111592..366f6b457e272 100644 --- a/system/system_diagnostic_graph/src/core/graph.hpp +++ b/system/system_diagnostic_graph/src/core/graph.hpp @@ -34,19 +34,16 @@ class Graph final Graph(); ~Graph(); - void init(const std::string & file, const std::string & mode); - void callback(const DiagnosticArray & array, const rclcpp::Time & stamp); - void update(const rclcpp::Time & stamp); - DiagnosticGraph message() const; - std::vector nodes() const; + void init(const std::string & file, const std::string & mode = ""); + void callback(const rclcpp::Time & stamp, const DiagnosticArray & array); + DiagnosticGraph report(const rclcpp::Time & stamp); + std::vector nodes() const; void debug(); private: - std::vector> nodes_; - std::unordered_map diags_; - UnknownNode * unknown_; - rclcpp::Time stamp_; + std::vector> nodes_; + std::unordered_map diags_; }; } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/modes.cpp b/system/system_diagnostic_graph/src/core/modes.cpp index 0ca18f84b0407..96944bd50f81a 100644 --- a/system/system_diagnostic_graph/src/core/modes.cpp +++ b/system/system_diagnostic_graph/src/core/modes.cpp @@ -15,7 +15,8 @@ #include "modes.hpp" #include "config.hpp" -#include "nodes.hpp" +#include "error.hpp" +#include "units.hpp" #include #include @@ -24,22 +25,22 @@ namespace system_diagnostic_graph { -OperationModes::OperationModes(rclcpp::Node & node, const std::vector & graph) +OperationModes::OperationModes(rclcpp::Node & node, const std::vector & graph) { pub_ = node.create_publisher("/system/operation_mode/availability", rclcpp::QoS(1)); - using PathNodes = std::unordered_map; - PathNodes paths; + using PathDict = std::unordered_map; + PathDict paths; for (const auto & node : graph) { paths[node->path()] = node; } - const auto find_node = [](const PathNodes & paths, const std::string & name) { + const auto find_node = [](const PathDict & paths, const std::string & name) { const auto iter = paths.find(name); if (iter != paths.end()) { return iter->second; } - throw ConfigError("summary node '" + name + "' does node exist"); + throw error("summary node is not found", name); }; // clang-format off @@ -55,7 +56,7 @@ OperationModes::OperationModes(rclcpp::Node & node, const std::vectorlevel() == DiagnosticStatus::OK; }; + const auto is_ok = [](const BaseUnit * node) { return node->level() == DiagnosticStatus::OK; }; // clang-format off Availability message; diff --git a/system/system_diagnostic_graph/src/core/modes.hpp b/system/system_diagnostic_graph/src/core/modes.hpp index ead1ec10dce93..47a31b8d2a152 100644 --- a/system/system_diagnostic_graph/src/core/modes.hpp +++ b/system/system_diagnostic_graph/src/core/modes.hpp @@ -29,7 +29,7 @@ namespace system_diagnostic_graph class OperationModes { public: - explicit OperationModes(rclcpp::Node & node, const std::vector & graph); + OperationModes(rclcpp::Node & node, const std::vector & graph); void update(const rclcpp::Time & stamp) const; private: @@ -37,13 +37,13 @@ class OperationModes rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher::SharedPtr pub_; - BaseNode * stop_mode_; - BaseNode * autonomous_mode_; - BaseNode * local_mode_; - BaseNode * remote_mode_; - BaseNode * emergency_stop_mrm_; - BaseNode * comfortable_stop_mrm_; - BaseNode * pull_over_mrm_; + BaseUnit * stop_mode_; + BaseUnit * autonomous_mode_; + BaseUnit * local_mode_; + BaseUnit * remote_mode_; + BaseUnit * emergency_stop_mrm_; + BaseUnit * comfortable_stop_mrm_; + BaseUnit * pull_over_mrm_; }; } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/nodes.cpp b/system/system_diagnostic_graph/src/core/nodes.cpp deleted file mode 100644 index bbc4bb4d42561..0000000000000 --- a/system/system_diagnostic_graph/src/core/nodes.cpp +++ /dev/null @@ -1,157 +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 "nodes.hpp" - -#include "exprs.hpp" - -#include - -#include -#include - -namespace system_diagnostic_graph -{ - -BaseNode::BaseNode(const std::string & path) : path_(path) -{ - index_ = 0; -} - -UnitNode::UnitNode(const std::string & path) : BaseNode(path) -{ - level_ = DiagnosticStatus::STALE; -} - -UnitNode::~UnitNode() -{ - // for unique_ptr -} - -void UnitNode::create(ConfigObject & config, ExprInit & exprs) -{ - expr_ = exprs.create(parse_expr_config(config)); -} - -void UnitNode::update(const rclcpp::Time &) -{ - const auto result = expr_->eval(); - level_ = result.level; - links_.clear(); - for (const auto & [node, used] : result.links) { - DiagnosticLink link; - link.index = node->index(); - link.used = used; - links_.push_back(link); - } -} - -DiagnosticNode UnitNode::report() const -{ - DiagnosticNode message; - message.status.level = level_; - message.status.name = path_; - message.links = links_; - return message; -} - -DiagnosticLevel UnitNode::level() const -{ - return level_; -} - -std::vector UnitNode::links() const -{ - return expr_->get_dependency(); -} - -DiagNode::DiagNode(const std::string & path, ConfigObject & config) : BaseNode(path) -{ - timeout_ = 3.0; // TODO(Takagi, Isamu): parameterize - name_ = config.take_text("name"); - - status_.level = DiagnosticStatus::STALE; -} - -void DiagNode::create(ConfigObject &, ExprInit &) -{ -} - -void DiagNode::update(const rclcpp::Time & stamp) -{ - if (time_) { - const auto elapsed = (stamp - time_.value()).seconds(); - if (timeout_ < elapsed) { - status_ = DiagnosticStatus(); - status_.level = DiagnosticStatus::STALE; - time_ = std::nullopt; - } - } -} - -DiagnosticNode DiagNode::report() const -{ - DiagnosticNode message; - message.status = status_; - message.status.name = path_; - return message; -} - -DiagnosticLevel DiagNode::level() const -{ - return status_.level; -} - -void DiagNode::callback(const DiagnosticStatus & status, const rclcpp::Time & stamp) -{ - status_ = status; - time_ = stamp; -} - -UnknownNode::UnknownNode(const std::string & path) : BaseNode(path) -{ -} - -void UnknownNode::create(ConfigObject &, ExprInit &) -{ -} - -void UnknownNode::update(const rclcpp::Time & stamp) -{ - (void)stamp; -} - -DiagnosticNode UnknownNode::report() const -{ - DiagnosticNode message; - message.status.name = path_; - for (const auto & diag : diagnostics_) { - diagnostic_msgs::msg::KeyValue kv; - kv.key = diag.first; - message.status.values.push_back(kv); - } - return message; -} - -DiagnosticLevel UnknownNode::level() const -{ - return DiagnosticStatus::WARN; -} - -void UnknownNode::callback(const DiagnosticStatus & status, const rclcpp::Time & stamp) -{ - diagnostics_[status.name] = stamp; -} - -} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/nodes.hpp b/system/system_diagnostic_graph/src/core/nodes.hpp deleted file mode 100644 index 1a849cf58b268..0000000000000 --- a/system/system_diagnostic_graph/src/core/nodes.hpp +++ /dev/null @@ -1,114 +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. - -#ifndef CORE__NODES_HPP_ -#define CORE__NODES_HPP_ - -#include "config.hpp" -#include "debug.hpp" -#include "types.hpp" - -#include - -#include -#include -#include -#include -#include -#include - -namespace system_diagnostic_graph -{ - -class BaseNode -{ -public: - explicit BaseNode(const std::string & path); - virtual ~BaseNode() = default; - virtual void create(ConfigObject & config, ExprInit & exprs) = 0; - virtual void update(const rclcpp::Time & stamp) = 0; - virtual DiagnosticNode report() const = 0; - virtual DiagnosticLevel level() const = 0; - virtual DiagDebugData debug() const = 0; - virtual std::vector links() const = 0; - - std::string path() const { return path_; } - - size_t index() const { return index_; } - void set_index(const size_t index) { index_ = index; } - -protected: - const std::string path_; - size_t index_; -}; - -class UnitNode : public BaseNode -{ -public: - explicit UnitNode(const std::string & path); - ~UnitNode() override; - void create(ConfigObject & config, ExprInit & exprs) override; - void update(const rclcpp::Time & stamp) override; - DiagnosticNode report() const override; - DiagnosticLevel level() const override; - DiagDebugData debug() const override; - std::vector links() const override; - -private: - std::unique_ptr expr_; - std::vector links_; - DiagnosticLevel level_; -}; - -class DiagNode : public BaseNode -{ -public: - DiagNode(const std::string & path, ConfigObject & config); - void create(ConfigObject & config, ExprInit & exprs) override; - void update(const rclcpp::Time & stamp) override; - DiagnosticNode report() const override; - DiagnosticLevel level() const override; - DiagDebugData debug() const override; - std::vector links() const override { return {}; } - std::string name() const { return name_; } - - void callback(const DiagnosticStatus & status, const rclcpp::Time & stamp); - -private: - double timeout_; - std::optional time_; - std::string name_; - DiagnosticStatus status_; -}; - -class UnknownNode : public BaseNode -{ -public: - explicit UnknownNode(const std::string & path); - void create(ConfigObject & config, ExprInit & exprs) override; - void update(const rclcpp::Time & stamp) override; - DiagnosticNode report() const override; - DiagnosticLevel level() const override; - DiagDebugData debug() const override; - std::vector links() const override { return {}; } - - void callback(const DiagnosticStatus & status, const rclcpp::Time & stamp); - -private: - std::map diagnostics_; -}; - -} // namespace system_diagnostic_graph - -#endif // CORE__NODES_HPP_ diff --git a/system/system_diagnostic_graph/src/core/types.hpp b/system/system_diagnostic_graph/src/core/types.hpp index 2adfbb3f9d4ef..c0b901d1e4511 100644 --- a/system/system_diagnostic_graph/src/core/types.hpp +++ b/system/system_diagnostic_graph/src/core/types.hpp @@ -31,13 +31,8 @@ using tier4_system_msgs::msg::DiagnosticLink; using tier4_system_msgs::msg::DiagnosticNode; using DiagnosticLevel = DiagnosticStatus::_level_type; -class BaseNode; -class UnitNode; -class DiagNode; -class UnknownNode; - -class BaseExpr; -class ExprInit; +class BaseUnit; +class DiagUnit; } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/units.cpp b/system/system_diagnostic_graph/src/core/units.cpp new file mode 100644 index 0000000000000..7cdd7c3e1266e --- /dev/null +++ b/system/system_diagnostic_graph/src/core/units.cpp @@ -0,0 +1,164 @@ +// 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 "units.hpp" + +#include +#include +#include +#include + +namespace system_diagnostic_graph +{ + +using LinkList = std::vector>; + +void merge(LinkList & a, const LinkList & b, bool uses) +{ + for (const auto & [node, used] : b) { + a.push_back(std::make_pair(node, used && uses)); + } +} + +auto resolve(const BaseUnit::NodeDict & dict, const std::vector & children) +{ + std::vector result; + for (const auto & child : children) { + result.push_back(dict.configs.at(child)); + } + return result; +} + +auto resolve(const BaseUnit::NodeDict & dict, const std::string & path) +{ + std::vector result; + result.push_back(dict.paths.at(path)); + return result; +} + +BaseUnit::BaseUnit(const std::string & path) : path_(path) +{ + index_ = 0; + level_ = DiagnosticStatus::OK; +} + +BaseUnit::NodeData BaseUnit::status() const +{ + if (path_.empty()) { + return {level_, links_}; + } else { + return {level_, {std::make_pair(this, true)}}; + } +} + +BaseUnit::NodeData BaseUnit::report() const +{ + return {level_, links_}; +} + +void DiagUnit::init(const UnitConfig::SharedPtr & config, const NodeDict &) +{ + timeout_ = 3.0; // TODO(Takagi, Isamu): parameterize + name_ = config->data.take_text("diag"); +} + +void DiagUnit::update(const rclcpp::Time & stamp) +{ + if (diagnostics_) { + const auto updated = diagnostics_.value().first; + const auto elapsed = (stamp - updated).seconds(); + if (timeout_ < elapsed) { + diagnostics_ = std::nullopt; + } + } + + if (diagnostics_) { + level_ = diagnostics_.value().second.level; + } else { + level_ = DiagnosticStatus::STALE; + } +} + +void DiagUnit::callback(const rclcpp::Time & stamp, const DiagnosticStatus & status) +{ + diagnostics_ = std::make_pair(stamp, status); +} + +AndUnit::AndUnit(const std::string & path, bool short_circuit) : BaseUnit(path) +{ + short_circuit_ = short_circuit; +} + +void AndUnit::init(const UnitConfig::SharedPtr & config, const NodeDict & dict) +{ + children_ = resolve(dict, config->children); +} + +void AndUnit::update(const rclcpp::Time &) +{ + if (children_.empty()) { + return; + } + + bool uses = true; + level_ = DiagnosticStatus::OK; + links_ = LinkList(); + + for (const auto & child : children_) { + const auto status = child->status(); + level_ = std::max(level_, status.level); + merge(links_, status.links, uses); + if (short_circuit_ && level_ != DiagnosticStatus::OK) { + uses = false; + } + } + level_ = std::min(level_, DiagnosticStatus::ERROR); +} + +void OrUnit::init(const UnitConfig::SharedPtr & config, const NodeDict & dict) +{ + children_ = resolve(dict, config->children); +} + +void OrUnit::update(const rclcpp::Time &) +{ + if (children_.empty()) { + return; + } + + level_ = DiagnosticStatus::ERROR; + links_ = LinkList(); + + for (const auto & child : children_) { + const auto status = child->status(); + level_ = std::min(level_, status.level); + merge(links_, status.links, true); + } + level_ = std::min(level_, DiagnosticStatus::ERROR); +} + +DebugUnit::DebugUnit(const std::string & path, DiagnosticLevel level) : BaseUnit(path) +{ + level_ = level; // overwrite +} + +void DebugUnit::init(const UnitConfig::SharedPtr &, const NodeDict &) +{ +} + +void DebugUnit::update(const rclcpp::Time &) +{ +} + +} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/units.hpp b/system/system_diagnostic_graph/src/core/units.hpp new file mode 100644 index 0000000000000..ad5fa4c4bc090 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/units.hpp @@ -0,0 +1,119 @@ +// 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 CORE__UNITS_HPP_ +#define CORE__UNITS_HPP_ + +#include "config.hpp" +#include "types.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +namespace system_diagnostic_graph +{ + +class BaseUnit +{ +public: + struct NodeDict + { + std::unordered_map configs; + std::unordered_map paths; + }; + struct NodeData + { + DiagnosticLevel level; + std::vector> links; + }; + using UniquePtr = std::unique_ptr; + using UniquePtrList = std::vector>; + + explicit BaseUnit(const std::string & path); + virtual ~BaseUnit() = default; + virtual void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) = 0; + virtual void update(const rclcpp::Time & stamp) = 0; + + NodeData status() const; + NodeData report() const; + DiagnosticLevel level() const { return level_; } + + auto path() const { return path_; } + auto children() const { return children_; } + + size_t index() const { return index_; } + void set_index(const size_t index) { index_ = index; } + +protected: + DiagnosticLevel level_; + std::string path_; + std::vector children_; + std::vector> links_; + +private: + size_t index_; +}; + +class DiagUnit : public BaseUnit +{ +public: + using BaseUnit::BaseUnit; + void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; + void update(const rclcpp::Time & stamp) override; + + std::string name() const { return name_; } + void callback(const rclcpp::Time & stamp, const DiagnosticStatus & status); + +private: + double timeout_; + std::optional> diagnostics_; + std::string name_; +}; + +class AndUnit : public BaseUnit +{ +public: + AndUnit(const std::string & path, bool short_circuit); + void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; + void update(const rclcpp::Time & stamp) override; + +private: + bool short_circuit_; +}; + +class OrUnit : public BaseUnit +{ +public: + using BaseUnit::BaseUnit; + void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; + void update(const rclcpp::Time & stamp) override; +}; + +class DebugUnit : public BaseUnit +{ +public: + DebugUnit(const std::string & path, DiagnosticLevel level); + void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; + void update(const rclcpp::Time & stamp) override; +}; + +} // namespace system_diagnostic_graph + +#endif // CORE__UNITS_HPP_ diff --git a/system/system_diagnostic_graph/src/main.cpp b/system/system_diagnostic_graph/src/main.cpp index 722ddcf833577..7db35d1fd2551 100644 --- a/system/system_diagnostic_graph/src/main.cpp +++ b/system/system_diagnostic_graph/src/main.cpp @@ -58,16 +58,14 @@ MainNode::~MainNode() void MainNode::on_timer() { const auto stamp = now(); - graph_.update(stamp); + pub_graph_->publish(graph_.report(stamp)); graph_.debug(); - pub_graph_->publish(graph_.message()); - if (modes_) modes_->update(stamp); } void MainNode::on_diag(const DiagnosticArray::ConstSharedPtr msg) { - graph_.callback(*msg, now()); + graph_.callback(now(), *msg); } } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/test/files/field-not-found.yaml b/system/system_diagnostic_graph/test/files/field-not-found.yaml new file mode 100644 index 0000000000000..9e2b3708c049a --- /dev/null +++ b/system/system_diagnostic_graph/test/files/field-not-found.yaml @@ -0,0 +1,2 @@ +nodes: + - test-type: test-type diff --git a/system/system_diagnostic_graph/test/files/file-not-found.yaml b/system/system_diagnostic_graph/test/files/file-not-found.yaml new file mode 100644 index 0000000000000..ffc83b42096f0 --- /dev/null +++ b/system/system_diagnostic_graph/test/files/file-not-found.yaml @@ -0,0 +1,2 @@ +files: + - { path: $(dirname)/fake-file-name.yaml } diff --git a/system/system_diagnostic_graph/test/files/graph-circulation.yaml b/system/system_diagnostic_graph/test/files/graph-circulation.yaml new file mode 100644 index 0000000000000..4c014bdcec3f9 --- /dev/null +++ b/system/system_diagnostic_graph/test/files/graph-circulation.yaml @@ -0,0 +1,9 @@ +nodes: + - path: /foo/bar + type: and + list: + - { type: link, link: /foo/baz } + - path: /foo/baz + type: and + list: + - { type: link, link: /foo/bar } diff --git a/system/system_diagnostic_graph/test/files/invalid-dict-type.yaml b/system/system_diagnostic_graph/test/files/invalid-dict-type.yaml new file mode 100644 index 0000000000000..6499b05ab8b7d --- /dev/null +++ b/system/system_diagnostic_graph/test/files/invalid-dict-type.yaml @@ -0,0 +1 @@ +nodes: test-object diff --git a/system/system_diagnostic_graph/test/files/invalid-list-type.yaml b/system/system_diagnostic_graph/test/files/invalid-list-type.yaml new file mode 100644 index 0000000000000..4fc5d96c08c62 --- /dev/null +++ b/system/system_diagnostic_graph/test/files/invalid-list-type.yaml @@ -0,0 +1,3 @@ +nodes: + - type: and + list: test-list diff --git a/system/system_diagnostic_graph/test/files/path-conflict.yaml b/system/system_diagnostic_graph/test/files/path-conflict.yaml new file mode 100644 index 0000000000000..ea3e536b74216 --- /dev/null +++ b/system/system_diagnostic_graph/test/files/path-conflict.yaml @@ -0,0 +1,5 @@ +nodes: + - path: /foo/bar + type: and + - path: /foo/bar + type: and diff --git a/system/system_diagnostic_graph/test/files/path-not-found.yaml b/system/system_diagnostic_graph/test/files/path-not-found.yaml new file mode 100644 index 0000000000000..6b0b10dfa94f4 --- /dev/null +++ b/system/system_diagnostic_graph/test/files/path-not-found.yaml @@ -0,0 +1,5 @@ +nodes: + - path: /foo/bar + type: and + - link: /foo/baz + type: link diff --git a/system/system_diagnostic_graph/test/files/unknown-node-type.yaml b/system/system_diagnostic_graph/test/files/unknown-node-type.yaml new file mode 100644 index 0000000000000..a3d66fbfa43cb --- /dev/null +++ b/system/system_diagnostic_graph/test/files/unknown-node-type.yaml @@ -0,0 +1,2 @@ +nodes: + - type: test-type diff --git a/system/system_diagnostic_graph/test/files/unknown-substitution.yaml b/system/system_diagnostic_graph/test/files/unknown-substitution.yaml new file mode 100644 index 0000000000000..7286321c30439 --- /dev/null +++ b/system/system_diagnostic_graph/test/files/unknown-substitution.yaml @@ -0,0 +1,2 @@ +files: + - { path: $(dummy) } diff --git a/system/system_diagnostic_graph/test/src/test.cpp b/system/system_diagnostic_graph/test/src/test.cpp new file mode 100644 index 0000000000000..b763179be0791 --- /dev/null +++ b/system/system_diagnostic_graph/test/src/test.cpp @@ -0,0 +1,88 @@ +// 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 "core/error.hpp" +#include "core/graph.hpp" + +#include + +#include +#include + +using namespace system_diagnostic_graph; // NOLINT(build/namespaces) + +std::filesystem::path resource(const std::string & path) +{ + return std::filesystem::path(TEST_RESOURCE_PATH) / path; +} + +TEST(ConfigFile, RootNotFound) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("fake-file-name.yaml")), FileNotFound); +} + +TEST(ConfigFile, FileNotFound) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("file-not-found.yaml")), FileNotFound); +} + +TEST(ConfigFile, UnknownSubstitution) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("unknown-substitution.yaml")), UnknownType); +} + +TEST(ConfigFile, UnknownNodeType) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("unknown-node-type.yaml")), UnknownType); +} + +TEST(ConfigFile, InvalidDictType) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("invalid-dict-type.yaml")), InvalidType); +} + +TEST(ConfigFile, InvalidListType) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("invalid-list-type.yaml")), InvalidType); +} + +TEST(ConfigFile, FieldNotFound) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("field-not-found.yaml")), FieldNotFound); +} + +TEST(ConfigFile, PathConflict) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("path-conflict.yaml")), PathConflict); +} + +TEST(ConfigFile, PathNotFound) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("path-not-found.yaml")), PathNotFound); +} + +TEST(ConfigFile, GraphCirculation) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("graph-circulation.yaml")), GraphStructure); +} diff --git a/system/system_diagnostic_graph/util/debug.py b/system/system_diagnostic_graph/util/debug.py deleted file mode 100755 index dac917a26a2f6..0000000000000 --- a/system/system_diagnostic_graph/util/debug.py +++ /dev/null @@ -1,85 +0,0 @@ -#!/usr/bin/env python3 - -# 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. - -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSProfile -from tier4_system_msgs.msg import DiagnosticGraph - - -class StructNode(Node): - def __init__(self): - super().__init__("system_diagnostic_graph_tools_struct") - qos_struct = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) - self.sub_struct = self.create_subscription( - DiagnosticGraph, "/diagnostics_graph/struct", self.callback, qos_struct - ) - self.message = None - - def callback(self, msg): - self.message = msg - - -class NodeSpinner: - def __init__(self, time): - self.time = time - self.wait = True - - def on_timer(self): - self.wait = False - - def spin(self, node): - timer = node.create_timer(self.time, self.on_timer) - while self.wait: - rclpy.spin_once(node) - node.destroy_timer(timer) - - -class GraphNode: - def __init__(self, msg): - self.name = msg.name - self.links = msg.links - - -class GraphStruct: - def __init__(self, msg): - self.nodes = [GraphNode(node) for node in msg.nodes] - - @staticmethod - def Subscribe(): - spin = NodeSpinner(1.0) - node = StructNode() - spin.spin(node) - return GraphStruct(node.message) - - def visualize(self): - from graphviz import Digraph - - graph = Digraph() - graph.attr("node", shape="box") - for node in self.nodes: - graph.node(node.name) - for link in node.links: - graph.edge(node.name, self.nodes[link].name) - graph.view() - - -if __name__ == "__main__": - rclpy.init() - graph = GraphStruct.Subscribe() - rclpy.shutdown() - graph.visualize() diff --git a/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml index f82f7d732f5dc..a0b13a899a6ba 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml @@ -29,3 +29,13 @@ path: localization_accuracy contains: ["localization: localization_error_monitor"] timeout: 1.0 + + # This diagnostic should ideally be avoided in terms of Fault Tree Analysis (FTA) compatibility. + # However, we may need this since the localization accuracy is still not reliable enough and may produce + # false positives. Thus, NOTE that this diagnostic should be removed in the future when the localization accuracy + # is reliable enough. + sensor_fusion_status: + type: diagnostic_aggregator/GenericAnalyzer + path: sensor_fusion_status + contains: ["localization: ekf_localizer"] + timeout: 1.0 diff --git a/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp b/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp index 42bd2a3ea9236..ec2a90c6b27e4 100644 --- a/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp +++ b/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp @@ -94,7 +94,7 @@ class ProcessMonitor : public rclcpp::Node * @param [out] command output command line * @return true if success to get command line name */ - bool getCommandLineFromPiD(const std::string & pid, std::string * command); + bool getCommandLineFromPiD(const std::string & pid, std::string & command); /** * @brief get top-rated processes diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index 6f58339f2ff4b..1173431b0f7c2 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -414,14 +414,22 @@ void ProcessMonitor::getHighMemoryProcesses(const std::string & output) getTopratedProcesses(&memory_tasks_, &p2); } -bool ProcessMonitor::getCommandLineFromPiD(const std::string & pid, std::string * command) +bool ProcessMonitor::getCommandLineFromPiD(const std::string & pid, std::string & command) { std::string commandLineFilePath = "/proc/" + pid + "/cmdline"; - std::ifstream commandFile(commandLineFilePath); + std::ifstream commandFile(commandLineFilePath, std::ios::in | std::ios::binary); + if (commandFile.is_open()) { - std::getline(commandFile, *command); + std::vector buffer; + std::copy( + std::istream_iterator(commandFile), std::istream_iterator(), + std::back_inserter(buffer)); commandFile.close(); - return true; + std::replace( + buffer.begin(), buffer.end(), '\0', + ' '); // 0x00 is used as delimiter in /cmdline instead of 0x20 (space) + command = std::string(buffer.begin(), buffer.end()); + return (buffer.size() > 0) ? true : false; // cmdline is empty if it is kernel process } else { return false; } @@ -478,7 +486,7 @@ void ProcessMonitor::getTopratedProcesses( std::string program_name; std::getline(stream, program_name); - bool flag_find_command_line = getCommandLineFromPiD(info.processId, &info.commandName); + bool flag_find_command_line = getCommandLineFromPiD(info.processId, info.commandName); if (!flag_find_command_line) { info.commandName = program_name; // if command line is not found, use program name instead diff --git a/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp b/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp index 7e42551454e24..a6a331a0e7324 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp @@ -85,6 +85,10 @@ bool CSVLoader::validateMap(const Map & map, const bool is_col_decent) bool CSVLoader::validateData(const Table & table, const std::string & csv_path) { + if (table.empty()) { + std::cerr << "The table is empty." << std::endl; + return false; + } if (table[0].size() < 2) { std::cerr << "Cannot read " << csv_path.c_str() << " CSV file should have at least 2 column" << std::endl;