diff --git a/.github/workflows/cancel-previous-workflows.yaml b/.github/workflows/cancel-previous-workflows.yaml index 1da4d24966de9..44983f7deadcb 100644 --- a/.github/workflows/cancel-previous-workflows.yaml +++ b/.github/workflows/cancel-previous-workflows.yaml @@ -8,7 +8,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Cancel previous runs - uses: styfle/cancel-workflow-action@0.12.0 + uses: styfle/cancel-workflow-action@0.12.1 with: workflow_id: all all_but_latest: true diff --git a/.github/workflows/check-build-depends.yaml b/.github/workflows/check-build-depends.yaml index c790c2132d71e..81618a1db0eea 100644 --- a/.github/workflows/check-build-depends.yaml +++ b/.github/workflows/check-build-depends.yaml @@ -20,7 +20,7 @@ jobs: build-depends-repos: build_depends.repos steps: - name: Check out repository - uses: actions/checkout@v4 + uses: actions/checkout@v3 - name: Remove exec_depend uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 diff --git a/.github/workflows/clang-tidy-differential.yaml b/.github/workflows/clang-tidy-differential.yaml index 26c5314d4b119..a59287a6d9b37 100644 --- a/.github/workflows/clang-tidy-differential.yaml +++ b/.github/workflows/clang-tidy-differential.yaml @@ -5,24 +5,29 @@ on: types: - opened - synchronize + - reopened - labeled jobs: - prevent-no-label-execution: - uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 + make-sure-label-is-present: + uses: autowarefoundation/autoware-github-actions/.github/workflows/make-sure-label-is-present.yaml@v1 with: label: tag:run-clang-tidy-differential clang-tidy-differential: - needs: prevent-no-label-execution - if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} + needs: make-sure-label-is-present + if: ${{ needs.make-sure-label-is-present.outputs.result == 'true' }} runs-on: ubuntu-latest container: ghcr.io/autowarefoundation/autoware:latest-prebuilt-cuda steps: - - name: Check out repository + - name: Set PR fetch depth + run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" + + - name: Checkout PR branch and all PR commits uses: actions/checkout@v4 with: - fetch-depth: 0 + ref: ${{ github.event.pull_request.head.sha }} + fetch-depth: ${{ env.PR_FETCH_DEPTH }} - name: Show disk space before the tasks run: df -h @@ -36,7 +41,7 @@ jobs: - name: Get modified files id: get-modified-files - uses: tj-actions/changed-files@v35 + uses: tj-actions/changed-files@v42 with: files: | **/*.cpp @@ -51,3 +56,6 @@ jobs: target-files: ${{ steps.get-modified-files.outputs.all_changed_files }} clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy build-depends-repos: build_depends.repos + + - name: Show disk space after the tasks + run: df -h diff --git a/.github/workflows/clang-tidy-pr-comments-manually.yaml b/.github/workflows/clang-tidy-pr-comments-manually.yaml index 87f939fe8b72f..9bccd972becde 100644 --- a/.github/workflows/clang-tidy-pr-comments-manually.yaml +++ b/.github/workflows/clang-tidy-pr-comments-manually.yaml @@ -11,7 +11,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v4 + uses: actions/checkout@v3 - name: Download analysis results run: | @@ -36,7 +36,7 @@ jobs: - name: Check out PR head if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} - uses: actions/checkout@v4 + uses: actions/checkout@v3 with: repository: ${{ steps.set-variables.outputs.pr-head-repo }} ref: ${{ steps.set-variables.outputs.pr-head-ref }} diff --git a/.github/workflows/clang-tidy-pr-comments.yaml b/.github/workflows/clang-tidy-pr-comments.yaml index 0f6db69dfedd4..baaa0fb8e7744 100644 --- a/.github/workflows/clang-tidy-pr-comments.yaml +++ b/.github/workflows/clang-tidy-pr-comments.yaml @@ -13,7 +13,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v4 + uses: actions/checkout@v3 - name: Download analysis results run: | @@ -37,7 +37,7 @@ jobs: - name: Check out PR head if: ${{ steps.check-fixes-yaml-existence.outputs.exists == 'true' }} - uses: actions/checkout@v4 + uses: actions/checkout@v3 with: repository: ${{ steps.set-variables.outputs.pr-head-repo }} ref: ${{ steps.set-variables.outputs.pr-head-ref }} diff --git a/.github/workflows/delete-closed-pr-docs.yaml b/.github/workflows/delete-closed-pr-docs.yaml index eb211766c49dd..b7b009fb00263 100644 --- a/.github/workflows/delete-closed-pr-docs.yaml +++ b/.github/workflows/delete-closed-pr-docs.yaml @@ -10,7 +10,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v4 + uses: actions/checkout@v3 with: fetch-depth: 0 diff --git a/.github/workflows/deploy-docs.yaml b/.github/workflows/deploy-docs.yaml index e5967b0d50c67..b48d70dbacb0c 100644 --- a/.github/workflows/deploy-docs.yaml +++ b/.github/workflows/deploy-docs.yaml @@ -30,7 +30,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Check out repository - uses: actions/checkout@v4 + uses: actions/checkout@v3 with: fetch-depth: 0 ref: ${{ github.event.pull_request.head.sha }} diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml index 33c00ee1066ae..c724885fcb3e4 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.github/workflows/pre-commit.yaml @@ -10,7 +10,7 @@ jobs: steps: - name: Generate token id: generate-token - uses: tibdex/github-app-token@v1 + uses: tibdex/github-app-token@v2 with: app_id: ${{ secrets.APP_ID }} private_key: ${{ secrets.PRIVATE_KEY }} diff --git a/.github/workflows/spell-check-daily.yaml b/.github/workflows/spell-check-daily.yaml new file mode 100644 index 0000000000000..dcee449abc728 --- /dev/null +++ b/.github/workflows/spell-check-daily.yaml @@ -0,0 +1,20 @@ +name: spell-check-daily + +on: + schedule: + - cron: 0 0 * * * + workflow_dispatch: + +jobs: + spell-check-daily: + runs-on: ubuntu-latest + steps: + - name: Check out repository + uses: actions/checkout@v4 + + - name: Run spell-check + uses: autowarefoundation/autoware-github-actions/spell-check@v1 + with: + local-cspell-json: .cspell.json + incremental-files-only: false + cspell-json-url: https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/.cspell.json diff --git a/.github/workflows/spell-check-differential.yaml b/.github/workflows/spell-check-differential.yaml new file mode 100644 index 0000000000000..ee9d451ba9b9e --- /dev/null +++ b/.github/workflows/spell-check-differential.yaml @@ -0,0 +1,17 @@ +name: spell-check-differential + +on: + pull_request: + +jobs: + spell-check-differential: + runs-on: ubuntu-latest + steps: + - name: Check out repository + uses: actions/checkout@v4 + + - name: Run spell-check + uses: autowarefoundation/autoware-github-actions/spell-check@v1 + with: + local-cspell-json: .cspell.json + cspell-json-url: https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/.cspell.json diff --git a/.github/workflows/sync-files.yaml b/.github/workflows/sync-files.yaml index 5025e6c8bd7a7..51e523b8031bf 100644 --- a/.github/workflows/sync-files.yaml +++ b/.github/workflows/sync-files.yaml @@ -18,7 +18,7 @@ jobs: steps: - name: Generate token id: generate-token - uses: tibdex/github-app-token@v1 + uses: tibdex/github-app-token@v2 with: app_id: ${{ secrets.APP_ID }} private_key: ${{ secrets.PRIVATE_KEY }} diff --git a/.github/workflows/update-codeowners-from-packages.yaml b/.github/workflows/update-codeowners-from-packages.yaml index 7898dfe09177a..8b3d2407fbc75 100644 --- a/.github/workflows/update-codeowners-from-packages.yaml +++ b/.github/workflows/update-codeowners-from-packages.yaml @@ -18,7 +18,7 @@ jobs: steps: - name: Generate token id: generate-token - uses: tibdex/github-app-token@v1 + uses: tibdex/github-app-token@v2 with: app_id: ${{ secrets.APP_ID }} private_key: ${{ secrets.PRIVATE_KEY }} diff --git a/.pre-commit-config-optional.yaml b/.pre-commit-config-optional.yaml index 3b43f9ae1139d..8c9345e15f064 100644 --- a/.pre-commit-config-optional.yaml +++ b/.pre-commit-config-optional.yaml @@ -1,6 +1,6 @@ repos: - repo: https://github.com/tcort/markdown-link-check - rev: v3.11.2 + rev: v3.12.2 hooks: - id: markdown-link-check args: [--quiet, --config=.markdown-link-check.json] diff --git a/common/tier4_adapi_rviz_plugin/CMakeLists.txt b/common/tier4_adapi_rviz_plugin/CMakeLists.txt index 1cf1ca95b852e..f06ab33823a3f 100644 --- a/common/tier4_adapi_rviz_plugin/CMakeLists.txt +++ b/common/tier4_adapi_rviz_plugin/CMakeLists.txt @@ -12,6 +12,7 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON) ament_auto_add_library(${PROJECT_NAME} SHARED src/route_tool.cpp src/route_panel.cpp + src/state_panel.cpp src/door_panel.cpp ) diff --git a/common/tier4_adapi_rviz_plugin/icons/classes/StatePanel.png b/common/tier4_adapi_rviz_plugin/icons/classes/StatePanel.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/tier4_adapi_rviz_plugin/icons/classes/StatePanel.png differ diff --git a/common/tier4_adapi_rviz_plugin/package.xml b/common/tier4_adapi_rviz_plugin/package.xml index 19f8857984354..3f8c34be318d1 100644 --- a/common/tier4_adapi_rviz_plugin/package.xml +++ b/common/tier4_adapi_rviz_plugin/package.xml @@ -13,6 +13,7 @@ autoware_cmake autoware_ad_api_specs + autoware_vehicle_msgs component_interface_utils geometry_msgs libqt5-core @@ -21,6 +22,8 @@ rclcpp rviz_common rviz_default_plugins + tier4_external_api_msgs + tier4_planning_msgs ament_lint_auto autoware_lint_common diff --git a/common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml b/common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml index 8b095c0e7d037..0b4f00bd56278 100644 --- a/common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml @@ -8,6 +8,10 @@ RoutePanel + + StatePanel + + DoorPanel diff --git a/common/tier4_adapi_rviz_plugin/src/state_panel.cpp b/common/tier4_adapi_rviz_plugin/src/state_panel.cpp new file mode 100644 index 0000000000000..982c8fa166b7d --- /dev/null +++ b/common/tier4_adapi_rviz_plugin/src/state_panel.cpp @@ -0,0 +1,637 @@ +// +// Copyright 2020 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. +// + +#include "state_panel.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include + +inline std::string Bool2String(const bool var) +{ + return var ? "True" : "False"; +} + +namespace tier4_adapi_rviz_plugins +{ + +StatePanel::StatePanel(QWidget * parent) : rviz_common::Panel(parent) +{ + // Gear + auto * gear_prefix_label_ptr = new QLabel("GEAR: "); + gear_prefix_label_ptr->setAlignment(Qt::AlignRight | Qt::AlignVCenter); + gear_label_ptr_ = new QLabel("INIT"); + gear_label_ptr_->setAlignment(Qt::AlignCenter); + auto * gear_layout = new QHBoxLayout; + gear_layout->addWidget(gear_prefix_label_ptr); + gear_layout->addWidget(gear_label_ptr_); + + // Velocity Limit + velocity_limit_button_ptr_ = new QPushButton("Send Velocity Limit"); + pub_velocity_limit_input_ = new QSpinBox(); + pub_velocity_limit_input_->setRange(-100.0, 100.0); + pub_velocity_limit_input_->setValue(0.0); + pub_velocity_limit_input_->setSingleStep(5.0); + connect(velocity_limit_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickVelocityLimit())); + + // Emergency Button + emergency_button_ptr_ = new QPushButton("Set Emergency"); + connect(emergency_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickEmergencyButton())); + + // Layout + auto * v_layout = new QVBoxLayout; + auto * velocity_limit_layout = new QHBoxLayout(); + v_layout->addWidget(makeOperationModeGroup()); + v_layout->addWidget(makeControlModeGroup()); + { + auto * h_layout = new QHBoxLayout(); + h_layout->addWidget(makeRoutingGroup()); + h_layout->addWidget(makeLocalizationGroup()); + h_layout->addWidget(makeMotionGroup()); + h_layout->addWidget(makeFailSafeGroup()); + v_layout->addLayout(h_layout); + } + + v_layout->addLayout(gear_layout); + velocity_limit_layout->addWidget(velocity_limit_button_ptr_); + velocity_limit_layout->addWidget(pub_velocity_limit_input_); + velocity_limit_layout->addWidget(new QLabel(" [km/h]")); + velocity_limit_layout->addWidget(emergency_button_ptr_); + v_layout->addLayout(velocity_limit_layout); + setLayout(v_layout); +} + +QGroupBox * StatePanel::makeOperationModeGroup() +{ + auto * group = new QGroupBox("OperationMode"); + auto * grid = new QGridLayout; + + operation_mode_label_ptr_ = new QLabel("INIT"); + operation_mode_label_ptr_->setAlignment(Qt::AlignCenter); + operation_mode_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(operation_mode_label_ptr_, 0, 0, 0, 1); + + auto_button_ptr_ = new QPushButton("AUTO"); + auto_button_ptr_->setCheckable(true); + connect(auto_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutonomous())); + grid->addWidget(auto_button_ptr_, 0, 1); + + stop_button_ptr_ = new QPushButton("STOP"); + stop_button_ptr_->setCheckable(true); + connect(stop_button_ptr_, SIGNAL(clicked()), SLOT(onClickStop())); + grid->addWidget(stop_button_ptr_, 0, 2); + + local_button_ptr_ = new QPushButton("LOCAL"); + local_button_ptr_->setCheckable(true); + connect(local_button_ptr_, SIGNAL(clicked()), SLOT(onClickLocal())); + grid->addWidget(local_button_ptr_, 1, 1); + + remote_button_ptr_ = new QPushButton("REMOTE"); + remote_button_ptr_->setCheckable(true); + connect(remote_button_ptr_, SIGNAL(clicked()), SLOT(onClickRemote())); + grid->addWidget(remote_button_ptr_, 1, 2); + + group->setLayout(grid); + return group; +} + +QGroupBox * StatePanel::makeControlModeGroup() +{ + auto * group = new QGroupBox("AutowareControl"); + auto * grid = new QGridLayout; + + control_mode_label_ptr_ = new QLabel("INIT"); + control_mode_label_ptr_->setAlignment(Qt::AlignCenter); + control_mode_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(control_mode_label_ptr_, 0, 0); + + enable_button_ptr_ = new QPushButton("Enable"); + enable_button_ptr_->setCheckable(true); + connect(enable_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutowareControl())); + grid->addWidget(enable_button_ptr_, 0, 1); + + disable_button_ptr_ = new QPushButton("Disable"); + disable_button_ptr_->setCheckable(true); + connect(disable_button_ptr_, SIGNAL(clicked()), SLOT(onClickDirectControl())); + grid->addWidget(disable_button_ptr_, 0, 2); + + group->setLayout(grid); + return group; +} + +QGroupBox * StatePanel::makeRoutingGroup() +{ + auto * group = new QGroupBox("Routing"); + auto * grid = new QGridLayout; + + routing_label_ptr_ = new QLabel("INIT"); + routing_label_ptr_->setAlignment(Qt::AlignCenter); + routing_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(routing_label_ptr_, 0, 0); + + clear_route_button_ptr_ = new QPushButton("Clear Route"); + clear_route_button_ptr_->setCheckable(true); + connect(clear_route_button_ptr_, SIGNAL(clicked()), SLOT(onClickClearRoute())); + grid->addWidget(clear_route_button_ptr_, 1, 0); + + group->setLayout(grid); + return group; +} + +QGroupBox * StatePanel::makeLocalizationGroup() +{ + auto * group = new QGroupBox("Localization"); + auto * grid = new QGridLayout; + + localization_label_ptr_ = new QLabel("INIT"); + localization_label_ptr_->setAlignment(Qt::AlignCenter); + localization_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(localization_label_ptr_, 0, 0); + + init_by_gnss_button_ptr_ = new QPushButton("Init by GNSS"); + connect(init_by_gnss_button_ptr_, SIGNAL(clicked()), SLOT(onClickInitByGnss())); + grid->addWidget(init_by_gnss_button_ptr_, 1, 0); + + group->setLayout(grid); + return group; +} + +QGroupBox * StatePanel::makeMotionGroup() +{ + auto * group = new QGroupBox("Motion"); + auto * grid = new QGridLayout; + + motion_label_ptr_ = new QLabel("INIT"); + motion_label_ptr_->setAlignment(Qt::AlignCenter); + motion_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(motion_label_ptr_, 0, 0); + + accept_start_button_ptr_ = new QPushButton("Accept Start"); + accept_start_button_ptr_->setCheckable(true); + connect(accept_start_button_ptr_, SIGNAL(clicked()), SLOT(onClickAcceptStart())); + grid->addWidget(accept_start_button_ptr_, 1, 0); + + group->setLayout(grid); + return group; +} + +QGroupBox * StatePanel::makeFailSafeGroup() +{ + auto * group = new QGroupBox("FailSafe"); + auto * grid = new QGridLayout; + + mrm_state_label_ptr_ = new QLabel("INIT"); + mrm_state_label_ptr_->setAlignment(Qt::AlignCenter); + mrm_state_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(mrm_state_label_ptr_, 0, 0); + + mrm_behavior_label_ptr_ = new QLabel("INIT"); + mrm_behavior_label_ptr_->setAlignment(Qt::AlignCenter); + mrm_behavior_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(mrm_behavior_label_ptr_, 1, 0); + + group->setLayout(grid); + return group; +} + +void StatePanel::onInitialize() +{ + using std::placeholders::_1; + + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + // Operation Mode + sub_operation_mode_ = raw_node_->create_subscription( + "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(), + std::bind(&StatePanel::onOperationMode, this, _1)); + + client_change_to_autonomous_ = + raw_node_->create_client("/api/operation_mode/change_to_autonomous"); + + client_change_to_stop_ = + raw_node_->create_client("/api/operation_mode/change_to_stop"); + + client_change_to_local_ = + raw_node_->create_client("/api/operation_mode/change_to_local"); + + client_change_to_remote_ = + raw_node_->create_client("/api/operation_mode/change_to_remote"); + + client_enable_autoware_control_ = + raw_node_->create_client("/api/operation_mode/enable_autoware_control"); + + client_enable_direct_control_ = + raw_node_->create_client("/api/operation_mode/disable_autoware_control"); + + // Routing + sub_route_ = raw_node_->create_subscription( + "/api/routing/state", rclcpp::QoS{1}.transient_local(), + std::bind(&StatePanel::onRoute, this, _1)); + + client_clear_route_ = raw_node_->create_client("/api/routing/clear_route"); + + // Localization + sub_localization_ = raw_node_->create_subscription( + "/api/localization/initialization_state", rclcpp::QoS{1}.transient_local(), + std::bind(&StatePanel::onLocalization, this, _1)); + + client_init_by_gnss_ = + raw_node_->create_client("/api/localization/initialize"); + + // Motion + sub_motion_ = raw_node_->create_subscription( + "/api/motion/state", rclcpp::QoS{1}.transient_local(), + std::bind(&StatePanel::onMotion, this, _1)); + + client_accept_start_ = raw_node_->create_client("/api/motion/accept_start"); + + // FailSafe + sub_mrm_ = raw_node_->create_subscription( + "/api/fail_safe/mrm_state", rclcpp::QoS{1}.transient_local(), + std::bind(&StatePanel::onMRMState, this, _1)); + + // Others + sub_gear_ = raw_node_->create_subscription( + "/vehicle/status/gear_status", 10, std::bind(&StatePanel::onShift, this, _1)); + + sub_emergency_ = raw_node_->create_subscription( + "/api/autoware/get/emergency", 10, std::bind(&StatePanel::onEmergencyStatus, this, _1)); + + client_emergency_stop_ = raw_node_->create_client( + "/api/autoware/set/emergency"); + + pub_velocity_limit_ = raw_node_->create_publisher( + "/planning/scenario_planning/max_velocity_default", rclcpp::QoS{1}.transient_local()); +} + +void StatePanel::onOperationMode(const OperationModeState::ConstSharedPtr msg) +{ + auto changeButtonState = [this]( + QPushButton * button, const bool is_desired_mode_available, + const uint8_t current_mode = OperationModeState::UNKNOWN, + const uint8_t desired_mode = OperationModeState::STOP) { + if (is_desired_mode_available && current_mode != desired_mode) { + activateButton(button); + } else { + deactivateButton(button); + } + }; + + QString text = ""; + QString style_sheet = ""; + // Operation Mode + switch (msg->mode) { + case OperationModeState::AUTONOMOUS: + text = "AUTONOMOUS"; + style_sheet = "background-color: #00FF00;"; // green + break; + + case OperationModeState::LOCAL: + text = "LOCAL"; + style_sheet = "background-color: #FFFF00;"; // yellow + break; + + case OperationModeState::REMOTE: + text = "REMOTE"; + style_sheet = "background-color: #FFFF00;"; // yellow + break; + + case OperationModeState::STOP: + text = "STOP"; + style_sheet = "background-color: #FFA500;"; // orange + break; + + default: + text = "UNKNOWN"; + style_sheet = "background-color: #FF0000;"; // red + break; + } + + if (msg->is_in_transition) { + text += "\n(TRANSITION)"; + } + + updateLabel(operation_mode_label_ptr_, text, style_sheet); + + // Control Mode + if (msg->is_autoware_control_enabled) { + updateLabel(control_mode_label_ptr_, "Enable", "background-color: #00FF00;"); // green + } else { + updateLabel(control_mode_label_ptr_, "Disable", "background-color: #FFFF00;"); // yellow + } + + // Button + changeButtonState( + auto_button_ptr_, msg->is_autonomous_mode_available, msg->mode, OperationModeState::AUTONOMOUS); + changeButtonState( + stop_button_ptr_, msg->is_stop_mode_available, msg->mode, OperationModeState::STOP); + changeButtonState( + local_button_ptr_, msg->is_local_mode_available, msg->mode, OperationModeState::LOCAL); + changeButtonState( + remote_button_ptr_, msg->is_remote_mode_available, msg->mode, OperationModeState::REMOTE); + + changeButtonState(enable_button_ptr_, !msg->is_autoware_control_enabled); + changeButtonState(disable_button_ptr_, msg->is_autoware_control_enabled); +} + +void StatePanel::onRoute(const RouteState::ConstSharedPtr msg) +{ + QString text = ""; + QString style_sheet = ""; + switch (msg->state) { + case RouteState::UNSET: + text = "UNSET"; + style_sheet = "background-color: #FFFF00;"; // yellow + break; + + case RouteState::SET: + text = "SET"; + style_sheet = "background-color: #00FF00;"; // green + break; + + case RouteState::ARRIVED: + text = "ARRIVED"; + style_sheet = "background-color: #FFA500;"; // orange + break; + + case RouteState::CHANGING: + text = "CHANGING"; + style_sheet = "background-color: #FFFF00;"; // yellow + break; + + default: + text = "UNKNOWN"; + style_sheet = "background-color: #FF0000;"; // red + break; + } + + updateLabel(routing_label_ptr_, text, style_sheet); + + if (msg->state == RouteState::SET) { + activateButton(clear_route_button_ptr_); + } else { + deactivateButton(clear_route_button_ptr_); + } +} + +void StatePanel::onLocalization(const LocalizationInitializationState::ConstSharedPtr msg) +{ + QString text = ""; + QString style_sheet = ""; + switch (msg->state) { + case LocalizationInitializationState::UNINITIALIZED: + text = "UNINITIALIZED"; + style_sheet = "background-color: #FFFF00;"; // yellow + break; + + case LocalizationInitializationState::INITIALIZING: + text = "INITIALIZING"; + style_sheet = "background-color: #FFA500;"; // orange + break; + + case LocalizationInitializationState::INITIALIZED: + text = "INITIALIZED"; + style_sheet = "background-color: #00FF00;"; // green + break; + + default: + text = "UNKNOWN"; + style_sheet = "background-color: #FF0000;"; // red + break; + } + + updateLabel(localization_label_ptr_, text, style_sheet); +} + +void StatePanel::onMotion(const MotionState::ConstSharedPtr msg) +{ + QString text = ""; + QString style_sheet = ""; + switch (msg->state) { + case MotionState::STARTING: + text = "STARTING"; + style_sheet = "background-color: #FFFF00;"; // yellow + break; + + case MotionState::STOPPED: + text = "STOPPED"; + style_sheet = "background-color: #FFA500;"; // orange + break; + + case MotionState::MOVING: + text = "MOVING"; + style_sheet = "background-color: #00FF00;"; // green + break; + + default: + text = "UNKNOWN"; + style_sheet = "background-color: #FF0000;"; // red + break; + } + + updateLabel(motion_label_ptr_, text, style_sheet); + + if (msg->state == MotionState::STARTING) { + activateButton(accept_start_button_ptr_); + } else { + deactivateButton(accept_start_button_ptr_); + } +} + +void StatePanel::onMRMState(const MRMState::ConstSharedPtr msg) +{ + // state + { + QString text = ""; + QString style_sheet = ""; + switch (msg->state) { + case MRMState::NONE: + text = "NONE"; + style_sheet = "background-color: #00FF00;"; // green + break; + + case MRMState::MRM_OPERATING: + text = "MRM_OPERATING"; + style_sheet = "background-color: #FFA500;"; // orange + break; + + case MRMState::MRM_SUCCEEDED: + text = "MRM_SUCCEEDED"; + style_sheet = "background-color: #FFFF00;"; // yellow + break; + + case MRMState::MRM_FAILED: + text = "MRM_FAILED"; + style_sheet = "background-color: #FF0000;"; // red + break; + + default: + text = "UNKNOWN"; + style_sheet = "background-color: #FF0000;"; // red + break; + } + + updateLabel(mrm_state_label_ptr_, text, style_sheet); + } + + // behavior + { + QString text = ""; + QString style_sheet = ""; + switch (msg->behavior) { + case MRMState::NONE: + text = "NONE"; + style_sheet = "background-color: #00FF00;"; // green + break; + + case MRMState::PULL_OVER: + text = "PULL_OVER"; + style_sheet = "background-color: #FFFF00;"; // yellow + break; + + case MRMState::COMFORTABLE_STOP: + text = "COMFORTABLE_STOP"; + style_sheet = "background-color: #FFFF00;"; // yellow + break; + + case MRMState::EMERGENCY_STOP: + text = "EMERGENCY_STOP"; + style_sheet = "background-color: #FFA500;"; // orange + break; + + default: + text = "UNKNOWN"; + style_sheet = "background-color: #FF0000;"; // red + break; + } + + updateLabel(mrm_behavior_label_ptr_, text, style_sheet); + } +} + +void StatePanel::onShift(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) +{ + switch (msg->report) { + case autoware_vehicle_msgs::msg::GearReport::PARK: + gear_label_ptr_->setText("PARKING"); + break; + case autoware_vehicle_msgs::msg::GearReport::REVERSE: + gear_label_ptr_->setText("REVERSE"); + break; + case autoware_vehicle_msgs::msg::GearReport::DRIVE: + gear_label_ptr_->setText("DRIVE"); + break; + case autoware_vehicle_msgs::msg::GearReport::NEUTRAL: + gear_label_ptr_->setText("NEUTRAL"); + break; + case autoware_vehicle_msgs::msg::GearReport::LOW: + gear_label_ptr_->setText("LOW"); + break; + } +} + +void StatePanel::onEmergencyStatus( + const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg) +{ + current_emergency_ = msg->emergency; + if (msg->emergency) { + emergency_button_ptr_->setText(QString::fromStdString("Clear Emergency")); + emergency_button_ptr_->setStyleSheet("background-color: #FF0000;"); + } else { + emergency_button_ptr_->setText(QString::fromStdString("Set Emergency")); + emergency_button_ptr_->setStyleSheet("background-color: #00FF00;"); + } +} + +void StatePanel::onClickVelocityLimit() +{ + auto velocity_limit = std::make_shared(); + velocity_limit->max_velocity = pub_velocity_limit_input_->value() / 3.6; + pub_velocity_limit_->publish(*velocity_limit); +} + +void StatePanel::onClickAutonomous() +{ + callServiceWithoutResponse(client_change_to_autonomous_); +} +void StatePanel::onClickStop() +{ + callServiceWithoutResponse(client_change_to_stop_); +} +void StatePanel::onClickLocal() +{ + callServiceWithoutResponse(client_change_to_local_); +} +void StatePanel::onClickRemote() +{ + callServiceWithoutResponse(client_change_to_remote_); +} +void StatePanel::onClickAutowareControl() +{ + callServiceWithoutResponse(client_enable_autoware_control_); +} +void StatePanel::onClickDirectControl() +{ + callServiceWithoutResponse(client_enable_direct_control_); +} + +void StatePanel::onClickClearRoute() +{ + callServiceWithoutResponse(client_clear_route_); +} + +void StatePanel::onClickInitByGnss() +{ + callServiceWithoutResponse(client_init_by_gnss_); +} + +void StatePanel::onClickAcceptStart() +{ + callServiceWithoutResponse(client_accept_start_); +} + +void StatePanel::onClickEmergencyButton() +{ + using tier4_external_api_msgs::msg::ResponseStatus; + using tier4_external_api_msgs::srv::SetEmergency; + + auto request = std::make_shared(); + request->emergency = !current_emergency_; + + RCLCPP_INFO(raw_node_->get_logger(), request->emergency ? "Set Emergency" : "Clear Emergency"); + + client_emergency_stop_->async_send_request( + request, [this](rclcpp::Client::SharedFuture result) { + const auto & response = result.get(); + if (response->status.code == ResponseStatus::SUCCESS) { + RCLCPP_INFO(raw_node_->get_logger(), "service succeeded"); + } else { + RCLCPP_WARN( + raw_node_->get_logger(), "service failed: %s", response->status.message.c_str()); + } + }); +} + +} // namespace tier4_adapi_rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(tier4_adapi_rviz_plugins::StatePanel, rviz_common::Panel) diff --git a/common/tier4_adapi_rviz_plugin/src/state_panel.hpp b/common/tier4_adapi_rviz_plugin/src/state_panel.hpp new file mode 100644 index 0000000000000..b30494772061e --- /dev/null +++ b/common/tier4_adapi_rviz_plugin/src/state_panel.hpp @@ -0,0 +1,206 @@ +// +// Copyright 2020 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. +// + +#ifndef STATE_PANEL_HPP_ +#define STATE_PANEL_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace tier4_adapi_rviz_plugins +{ + +class StatePanel : public rviz_common::Panel +{ + using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; + using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; + using RouteState = autoware_adapi_v1_msgs::msg::RouteState; + using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute; + using LocalizationInitializationState = + autoware_adapi_v1_msgs::msg::LocalizationInitializationState; + using InitializeLocalization = autoware_adapi_v1_msgs::srv::InitializeLocalization; + using MotionState = autoware_adapi_v1_msgs::msg::MotionState; + using AcceptStart = autoware_adapi_v1_msgs::srv::AcceptStart; + using MRMState = autoware_adapi_v1_msgs::msg::MrmState; + + Q_OBJECT + +public: + explicit StatePanel(QWidget * parent = nullptr); + void onInitialize() override; + +public Q_SLOTS: // NOLINT for Qt + void onClickAutonomous(); + void onClickStop(); + void onClickLocal(); + void onClickRemote(); + void onClickAutowareControl(); + void onClickDirectControl(); + void onClickClearRoute(); + void onClickInitByGnss(); + void onClickAcceptStart(); + void onClickVelocityLimit(); + void onClickEmergencyButton(); + +protected: + // Layout + QGroupBox * makeOperationModeGroup(); + QGroupBox * makeControlModeGroup(); + QGroupBox * makeRoutingGroup(); + QGroupBox * makeLocalizationGroup(); + QGroupBox * makeMotionGroup(); + QGroupBox * makeFailSafeGroup(); + + void onShift(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); + void onEmergencyStatus(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg); + + rclcpp::Node::SharedPtr raw_node_; + rclcpp::Subscription::SharedPtr sub_gear_; + rclcpp::Client::SharedPtr client_emergency_stop_; + rclcpp::Subscription::SharedPtr sub_emergency_; + rclcpp::Publisher::SharedPtr pub_velocity_limit_; + + // Operation Mode + QLabel * operation_mode_label_ptr_{nullptr}; + QPushButton * stop_button_ptr_{nullptr}; + QPushButton * auto_button_ptr_{nullptr}; + QPushButton * local_button_ptr_{nullptr}; + QPushButton * remote_button_ptr_{nullptr}; + + rclcpp::Subscription::SharedPtr sub_operation_mode_; + rclcpp::Client::SharedPtr client_change_to_autonomous_; + rclcpp::Client::SharedPtr client_change_to_stop_; + rclcpp::Client::SharedPtr client_change_to_local_; + rclcpp::Client::SharedPtr client_change_to_remote_; + + // Control Mode + QLabel * control_mode_label_ptr_{nullptr}; + QPushButton * enable_button_ptr_{nullptr}; + QPushButton * disable_button_ptr_{nullptr}; + rclcpp::Client::SharedPtr client_enable_autoware_control_; + rclcpp::Client::SharedPtr client_enable_direct_control_; + + // Functions + void onOperationMode(const OperationModeState::ConstSharedPtr msg); + void changeOperationMode(const rclcpp::Client::SharedPtr client); + + // Routing + QLabel * routing_label_ptr_{nullptr}; + QPushButton * clear_route_button_ptr_{nullptr}; + + rclcpp::Subscription::SharedPtr sub_route_; + rclcpp::Client::SharedPtr client_clear_route_; + + void onRoute(const RouteState::ConstSharedPtr msg); + + // Localization + QLabel * localization_label_ptr_{nullptr}; + QPushButton * init_by_gnss_button_ptr_{nullptr}; + + rclcpp::Subscription::SharedPtr sub_localization_; + rclcpp::Client::SharedPtr client_init_by_gnss_; + + void onLocalization(const LocalizationInitializationState::ConstSharedPtr msg); + + // Motion + QLabel * motion_label_ptr_{nullptr}; + QPushButton * accept_start_button_ptr_{nullptr}; + + rclcpp::Subscription::SharedPtr sub_motion_; + rclcpp::Client::SharedPtr client_accept_start_; + + void onMotion(const MotionState::ConstSharedPtr msg); + + // FailSafe + QLabel * mrm_state_label_ptr_{nullptr}; + QLabel * mrm_behavior_label_ptr_{nullptr}; + + rclcpp::Subscription::SharedPtr sub_mrm_; + + void onMRMState(const MRMState::ConstSharedPtr msg); + + // Others + QPushButton * velocity_limit_button_ptr_; + QLabel * gear_label_ptr_; + + QSpinBox * pub_velocity_limit_input_; + QPushButton * emergency_button_ptr_; + + bool current_emergency_{false}; + + template + void callServiceWithoutResponse(const typename rclcpp::Client::SharedPtr client) + { + auto req = std::make_shared(); + + RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); + + if (!client->service_is_ready()) { + RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); + return; + } + + client->async_send_request(req, [this](typename rclcpp::Client::SharedFuture result) { + RCLCPP_DEBUG( + raw_node_->get_logger(), "Status: %d, %s", result.get()->status.code, + result.get()->status.message.c_str()); + }); + } + + static void updateLabel(QLabel * label, QString text, QString style_sheet) + { + label->setText(text); + label->setStyleSheet(style_sheet); + } + + static void activateButton(QAbstractButton * button) + { + button->setChecked(false); + button->setEnabled(true); + } + + static void deactivateButton(QAbstractButton * button) + { + button->setChecked(true); + button->setEnabled(false); + } +}; + +} // namespace tier4_adapi_rviz_plugins + +#endif // STATE_PANEL_HPP_ diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index 13f77aa47ff8c..2f3f5f08d4156 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -169,6 +169,7 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t | 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 | +| use_object_velocity_calculation | [-] | bool | flag to use the object velocity calculation. If set to false, object velocity is set to 0 [m/s] | true | | detection_range_min_height | [m] | double | minimum hight of detection range used for avoiding the ghost brake by false positive point clouds | 0.0 | | detection_range_max_height_margin | [m] | double | margin for maximum hight of detection range used for avoiding the ghost brake by false positive point clouds. `detection_range_max_height = vehicle_height + detection_range_max_height_margin` | 0.0 | | voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 | diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 68a42383feb1f..4233f8b6bebcb 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -3,6 +3,7 @@ # Ego path calculation use_predicted_trajectory: true use_imu_path: false + use_object_velocity_calculation: true min_generated_path_length: 0.5 imu_prediction_time_horizon: 1.5 imu_prediction_time_interval: 0.1 diff --git a/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp index a53d3c6289559..85d20aa27a4f5 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp @@ -312,6 +312,7 @@ class AEB : public rclcpp::Node bool publish_debug_pointcloud_; bool use_predicted_trajectory_; bool use_imu_path_; + bool use_object_velocity_calculation_; double path_footprint_extra_margin_; double detection_range_min_height_; double detection_range_max_height_margin_; diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 1d977b58e9ceb..945d8508cbc05 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -120,6 +120,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) publish_debug_pointcloud_ = declare_parameter("publish_debug_pointcloud"); use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); use_imu_path_ = declare_parameter("use_imu_path"); + use_object_velocity_calculation_ = declare_parameter("use_object_velocity_calculation"); path_footprint_extra_margin_ = declare_parameter("path_footprint_extra_margin"); detection_range_min_height_ = declare_parameter("detection_range_min_height"); detection_range_max_height_margin_ = @@ -167,6 +168,8 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( updateParam(parameters, "publish_debug_pointcloud", publish_debug_pointcloud_); updateParam(parameters, "use_predicted_trajectory", use_predicted_trajectory_); updateParam(parameters, "use_imu_path", use_imu_path_); + updateParam( + parameters, "use_object_velocity_calculation", use_object_velocity_calculation_); updateParam(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_); updateParam(parameters, "detection_range_min_height", detection_range_min_height_); updateParam( @@ -397,8 +400,10 @@ bool AEB::checkCollision(MarkerArray & debug_markers) if (closest_object_point_itr == objects_from_point_clusters.end()) { return std::nullopt; } - const auto closest_object_speed = collision_data_keeper_.calcObjectSpeedFromHistory( - *closest_object_point_itr, path, current_v); + const auto closest_object_speed = (use_object_velocity_calculation_) + ? collision_data_keeper_.calcObjectSpeedFromHistory( + *closest_object_point_itr, path, current_v) + : std::make_optional(0.0); if (!closest_object_speed.has_value()) { return std::nullopt; } 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 225c7ab9d1146..e3ee69fb02b20 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 @@ -3,7 +3,7 @@ - + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index d0046acf294b5..150744bbae4a7 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -18,8 +18,8 @@ ament_cmake_auto autoware_cmake - ar_tag_based_localizer automatic_pose_initializer + autoware_ar_tag_based_localizer eagleye_geo_pose_fusion eagleye_gnss_converter eagleye_rt diff --git a/localization/landmark_based_localizer/README.md b/localization/autoware_landmark_based_localizer/README.md similarity index 100% rename from localization/landmark_based_localizer/README.md rename to localization/autoware_landmark_based_localizer/README.md diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/CMakeLists.txt similarity index 96% rename from localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/CMakeLists.txt index a8435fa056847..8b2487ef79ea9 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(ar_tag_based_localizer) +project(autoware_ar_tag_based_localizer) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/README.md similarity index 100% rename from localization/landmark_based_localizer/ar_tag_based_localizer/README.md rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/README.md diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml similarity index 100% rename from localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/ar_tag_image.png b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/doc_image/ar_tag_image.png similarity index 100% rename from localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/ar_tag_image.png rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/doc_image/ar_tag_image.png diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result.png b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/doc_image/sample_result.png similarity index 100% rename from localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result.png rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/doc_image/sample_result.png diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/doc_image/sample_result_in_awsim.png similarity index 100% rename from localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/doc_image/sample_result_in_awsim.png diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml similarity index 82% rename from localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml index 34602ca70daf4..dd9956acce748 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml @@ -1,5 +1,5 @@ - + @@ -14,7 +14,7 @@ - + diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml similarity index 88% rename from localization/landmark_based_localizer/ar_tag_based_localizer/package.xml rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml index 90fdd2fee31f4..bf40719d7a5e6 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml @@ -1,9 +1,9 @@ - ar_tag_based_localizer + autoware_ar_tag_based_localizer 0.0.0 - The ar_tag_based_localizer package + The autoware_ar_tag_based_localizer package Shintaro Sakoda Masahiro Sakamoto Yamato Ando @@ -18,9 +18,9 @@ ament_index_cpp aruco + autoware_landmark_manager cv_bridge diagnostic_msgs - landmark_manager lanelet2_extension localization_util rclcpp diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json similarity index 100% rename from localization/landmark_based_localizer/ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/schema/ar_tag_based_localizer.schema.json diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp similarity index 100% rename from localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.cpp diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.hpp similarity index 98% rename from localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.hpp index f00621b3c520a..9826c04e3a86f 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/src/ar_tag_based_localizer.hpp @@ -45,7 +45,7 @@ #ifndef AR_TAG_BASED_LOCALIZER_HPP_ #define AR_TAG_BASED_LOCALIZER_HPP_ -#include "landmark_manager/landmark_manager.hpp" +#include "autoware/landmark_manager/landmark_manager.hpp" #include "localization_util/smart_pose_buffer.hpp" #include diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/test/test.cpp similarity index 96% rename from localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp rename to localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/test/test.cpp index d62c2b38b6cd1..295737ed723a5 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/test/test.cpp @@ -30,7 +30,7 @@ class TestArTagBasedLocalizer : public ::testing::Test void SetUp() override { const std::string yaml_path = - ament_index_cpp::get_package_share_directory("ar_tag_based_localizer") + + ament_index_cpp::get_package_share_directory("autoware_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()); diff --git a/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/CMakeLists.txt similarity index 92% rename from localization/landmark_based_localizer/landmark_manager/CMakeLists.txt rename to localization/autoware_landmark_based_localizer/autoware_landmark_manager/CMakeLists.txt index 1b57e5cc89018..6c67ff7a320e8 100644 --- a/localization/landmark_based_localizer/landmark_manager/CMakeLists.txt +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(landmark_manager) +project(autoware_landmark_manager) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/include/autoware/landmark_manager/landmark_manager.hpp similarity index 91% rename from localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp rename to localization/autoware_landmark_based_localizer/autoware_landmark_manager/include/autoware/landmark_manager/landmark_manager.hpp index 7c1a3f656dd66..3fc6505e72c31 100644 --- a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/include/autoware/landmark_manager/landmark_manager.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ -#define LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ +#ifndef AUTOWARE__LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ +#define AUTOWARE__LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ #include "lanelet2_extension/localization/landmark.hpp" @@ -61,4 +61,4 @@ class LandmarkManager } // namespace landmark_manager -#endif // LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ +#endif // AUTOWARE__LANDMARK_MANAGER__LANDMARK_MANAGER_HPP_ diff --git a/localization/landmark_based_localizer/landmark_manager/package.xml b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml similarity index 91% rename from localization/landmark_based_localizer/landmark_manager/package.xml rename to localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml index 4e080c0362c07..1471b5182def8 100644 --- a/localization/landmark_based_localizer/landmark_manager/package.xml +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml @@ -1,9 +1,9 @@ - landmark_manager + autoware_landmark_manager 0.0.0 - The landmark_manager package + The autoware_landmark_manager package Shintaro Sakoda Masahiro Sakamoto Yamato Ando diff --git a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp similarity index 99% rename from localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp rename to localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp index a808a6428f682..3953b3ba130d9 100644 --- a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/src/landmark_manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "landmark_manager/landmark_manager.hpp" +#include "autoware/landmark_manager/landmark_manager.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include "localization_util/util_func.hpp" diff --git a/localization/landmark_based_localizer/doc_image/consider_orientation.drawio.svg b/localization/autoware_landmark_based_localizer/doc_image/consider_orientation.drawio.svg similarity index 100% rename from localization/landmark_based_localizer/doc_image/consider_orientation.drawio.svg rename to localization/autoware_landmark_based_localizer/doc_image/consider_orientation.drawio.svg diff --git a/localization/landmark_based_localizer/doc_image/lanelet2_data_structure.drawio.svg b/localization/autoware_landmark_based_localizer/doc_image/lanelet2_data_structure.drawio.svg similarity index 100% rename from localization/landmark_based_localizer/doc_image/lanelet2_data_structure.drawio.svg rename to localization/autoware_landmark_based_localizer/doc_image/lanelet2_data_structure.drawio.svg diff --git a/localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg b/localization/autoware_landmark_based_localizer/doc_image/node_diagram.drawio.svg similarity index 100% rename from localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg rename to localization/autoware_landmark_based_localizer/doc_image/node_diagram.drawio.svg diff --git a/localization/landmark_based_localizer/doc_image/principle.png b/localization/autoware_landmark_based_localizer/doc_image/principle.png similarity index 100% rename from localization/landmark_based_localizer/doc_image/principle.png rename to localization/autoware_landmark_based_localizer/doc_image/principle.png diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/histogram.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/histogram.hpp index c3ac2475f7e08..18b06b08cb55e 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/histogram.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/histogram.hpp @@ -27,7 +27,7 @@ struct Histogram EIGEN_MAKE_ALIGNED_OPERATOR_NEW explicit Histogram(int bin = 10) : bin(bin) { data = Eigen::MatrixXf::Zero(3, bin); } - Eigen::MatrixXf eval() const + [[nodiscard]] Eigen::MatrixXf eval() const { float sum = data.sum(); if (sum < 1e-6f) throw std::runtime_error("invalid division"); @@ -37,7 +37,9 @@ struct Histogram void add(const cv::Vec3b & rgb) { for (int ch = 0; ch < 3; ++ch) { - int index = std::clamp(static_cast(rgb[ch] * bin / 255.f), 0, bin - 1); + int index = std::clamp( + static_cast(static_cast(rgb[ch]) * static_cast(bin) / 255.f), 0, + bin - 1); data(ch, index) += 1.0f; } } diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp index 7d644376ba591..d6d737a2808c4 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp @@ -76,7 +76,7 @@ class Lanelet2Overlay : public rclcpp::Node void draw_overlay( const cv::Mat & image, const std::optional & pose, const rclcpp::Time & stamp); void draw_overlay_line_segments( - cv::Mat & image, const Pose & pose, const LineSegments & line_segments); + cv::Mat & image, const Pose & pose, const LineSegments & near_segments); void make_vis_marker(const LineSegments & ls, const Pose & pose, const rclcpp::Time & stamp); }; diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp index 761c581200369..7da51cc5daf35 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp @@ -51,8 +51,8 @@ class LineSegmentDetector : public rclcpp::Node cv::Ptr line_segment_detector_; - std::vector remove_too_outer_elements( - const cv::Mat & lines, const cv::Size & size) const; + static std::vector remove_too_outer_elements( + const cv::Mat & lines, const cv::Size & size); void on_image(const sensor_msgs::msg::Image & msg); void execute(const cv::Mat & image, const rclcpp::Time & stamp); }; diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp index c8f036f2b12de..6b76db6751d26 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp @@ -63,14 +63,14 @@ class SegmentFilter : public rclcpp::Node bool define_project_func(); pcl::PointCloud project_lines( - const pcl::PointCloud & lines, const std::set & indices, + const pcl::PointCloud & points, const std::set & indices, bool negative = false) const; - std::set filter_by_mask( + static std::set filter_by_mask( const cv::Mat & mask, const pcl::PointCloud & edges); cv::Point2i to_cv_point(const Eigen::Vector3f & v) const; - void execute(const PointCloud2 & msg1, const Image & msg2); + void execute(const PointCloud2 & line_segments_msg, const Image & segment_msg); bool is_near_element(const pcl::PointNormal & pn, pcl::PointNormal & truncated_pn) const; }; diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp index f8d4c74dd5bf8..4cc2a9de4acc7 100644 --- a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp @@ -25,8 +25,9 @@ namespace yabloc::graph_segment { GraphSegment::GraphSegment(const rclcpp::NodeOptions & options) : Node("graph_segment", options), - target_height_ratio_(declare_parameter("target_height_ratio")), - target_candidate_box_width_(declare_parameter("target_candidate_box_width")) + target_height_ratio_(static_cast(declare_parameter("target_height_ratio"))), + target_candidate_box_width_( + static_cast(declare_parameter("target_candidate_box_width"))) { using std::placeholders::_1; @@ -38,8 +39,8 @@ GraphSegment::GraphSegment(const rclcpp::NodeOptions & options) pub_debug_image_ = create_publisher("~/debug/segmented_image", 10); const double sigma = declare_parameter("sigma"); - const float k = declare_parameter("k"); - const int min_size = declare_parameter("min_size"); + const float k = static_cast(declare_parameter("k")); + const int min_size = static_cast(declare_parameter("min_size")); segmentation_ = cv::ximgproc::segmentation::createGraphSegmentation(sigma, k, min_size); // additional area pickup module @@ -52,16 +53,20 @@ GraphSegment::GraphSegment(const rclcpp::NodeOptions & options) cv::Vec3b random_hsv(int index) { // It generates colors that are not too bright or too vivid, but rich in hues. - double base = static_cast(index); - return cv::Vec3b(std::fmod(base * 0.7, 1.0) * 180, 0.7 * 255, 0.5 * 255); + auto base = static_cast(index); + return { + static_cast(std::fmod(base * 0.7, 1.0) * 180), + static_cast(0.7 * 255), static_cast(0.5 * 255)}; }; int GraphSegment::search_most_road_like_class(const cv::Mat & segmented) const { - const int W = target_candidate_box_width_; - const float R = target_height_ratio_; - cv::Point2i target_px(segmented.cols * 0.5, segmented.rows * R); - cv::Rect2i rect(target_px + cv::Point2i(-W, -W), target_px + cv::Point2i(W, W)); + const int bw = target_candidate_box_width_; + const float r = target_height_ratio_; + cv::Point2i target_px( + static_cast(static_cast(segmented.cols) * 0.5), + static_cast(static_cast(segmented.rows) * r)); + cv::Rect2i rect(target_px + cv::Point2i(-bw, -bw), target_px + cv::Point2i(bw, bw)); std::unordered_map areas; std::unordered_set candidates; @@ -69,7 +74,7 @@ int GraphSegment::search_most_road_like_class(const cv::Mat & segmented) const const int * seg_ptr = segmented.ptr(h); for (int w = 0; w < segmented.cols; w++) { int key = seg_ptr[w]; - if (areas.count(key) == 0) areas[key] = 0; + areas.try_emplace(key, 0); areas[key]++; if (rect.contains(cv::Point2i{w, h})) candidates.insert(key); } @@ -111,8 +116,8 @@ void GraphSegment::on_image(const Image & msg) cv::Mat debug_image = cv::Mat::zeros(resized.size(), CV_8UC3); for (int h = 0; h < resized.rows; h++) { // NOTE: Accessing through ptr() is faster than at() - cv::Vec3b * const debug_image_ptr = debug_image.ptr(h); - uchar * const output_image_ptr = output_image.ptr(h); + auto * const debug_image_ptr = debug_image.ptr(h); + auto * const output_image_ptr = output_image.ptr(h); const int * const segmented_image_ptr = segmented.ptr(h); for (int w = 0; w < resized.cols; w++) { @@ -148,10 +153,10 @@ void GraphSegment::draw_and_publish_image( // Draw target rectangle { - const int W = target_candidate_box_width_; - const float R = target_height_ratio_; - cv::Point2i target(size.width / 2, size.height * R); - cv::Rect2i rect(target + cv::Point2i(-W, -W), target + cv::Point2i(W, W)); + const int w = target_candidate_box_width_; + const float r = target_height_ratio_; + cv::Point2i target(size.width / 2, static_cast(static_cast(size.height) * r)); + cv::Rect2i rect(target + cv::Point2i(-w, -w), target + cv::Point2i(w, w)); cv::rectangle(show_image, rect, cv::Scalar::all(0), 2); } diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/similar_area_searcher.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/similar_area_searcher.cpp index 85ce9e354362a..c7bf41bff459b 100644 --- a/localization/yabloc/yabloc_image_processing/src/graph_segment/similar_area_searcher.cpp +++ b/localization/yabloc/yabloc_image_processing/src/graph_segment/similar_area_searcher.cpp @@ -37,7 +37,7 @@ std::set SimilarAreaSearcher::search( for (int h = 0; h < rgb_image.rows; h++) { const int * seg_ptr = segmented.ptr(h); - const cv::Vec3b * rgb_ptr = rgb_image.ptr(h); + const auto * rgb_ptr = rgb_image.ptr(h); for (int w = 0; w < rgb_image.cols; w++) { int key = seg_ptr[w]; diff --git a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp index 107d861364038..d31b19406e320 100644 --- a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp @@ -72,11 +72,11 @@ void Lanelet2Overlay::on_image(const sensor_msgs::msg::Image & msg) // Search synchronized pose float min_abs_dt = std::numeric_limits::max(); std::optional synched_pose{std::nullopt}; - for (auto pose : pose_buffer_) { + for (const auto & pose : pose_buffer_) { auto dt = (rclcpp::Time(pose.header.stamp) - stamp); auto abs_dt = std::abs(dt.seconds()); if (abs_dt < min_abs_dt) { - min_abs_dt = abs_dt; + min_abs_dt = static_cast(abs_dt); synched_pose = pose.pose; } } @@ -93,19 +93,17 @@ void Lanelet2Overlay::on_line_segments(const PointCloud2 & msg) // Search synchronized pose float min_dt = std::numeric_limits::max(); geometry_msgs::msg::PoseStamped synched_pose; - for (auto pose : pose_buffer_) { + for (const auto & pose : pose_buffer_) { auto dt = (rclcpp::Time(pose.header.stamp) - stamp); auto abs_dt = std::abs(dt.seconds()); if (abs_dt < min_dt) { - min_dt = abs_dt; + min_dt = static_cast(abs_dt); synched_pose = pose; } } if (min_dt > 0.1) return; auto latest_pose_stamp = rclcpp::Time(pose_buffer_.back().header.stamp); - std::vector a; - LineSegments line_segments_cloud; pcl::fromROSMsg(msg, line_segments_cloud); make_vis_marker(line_segments_cloud, synched_pose.pose, stamp); @@ -139,32 +137,35 @@ void Lanelet2Overlay::draw_overlay_line_segments( if (!camera_extrinsic_.has_value()) return; if (!info_.has_value()) return; - Eigen::Matrix3f K = + Eigen::Matrix3f k = Eigen::Map >(info_->k.data()).cast().transpose(); - Eigen::Affine3f T = camera_extrinsic_.value(); + Eigen::Affine3f t = camera_extrinsic_.value(); Eigen::Affine3f transform = ground_plane_.align_with_slope(common::pose_to_affine(pose)); - auto projectLineSegment = - [K, T, transform]( + auto project_line_segment = + [k, t, transform]( const Eigen::Vector3f & p1, const Eigen::Vector3f & p2) -> std::tuple { - Eigen::Vector3f from_camera1 = K * T.inverse() * transform.inverse() * p1; - Eigen::Vector3f from_camera2 = K * T.inverse() * transform.inverse() * p2; - constexpr float EPSILON = 0.1f; - bool p1_is_visible = from_camera1.z() > EPSILON; - bool p2_is_visible = from_camera2.z() > EPSILON; + Eigen::Vector3f from_camera1 = k * t.inverse() * transform.inverse() * p1; + Eigen::Vector3f from_camera2 = k * t.inverse() * transform.inverse() * p2; + constexpr float epsilon = 0.1f; + bool p1_is_visible = from_camera1.z() > epsilon; + bool p2_is_visible = from_camera2.z() > epsilon; if ((!p1_is_visible) && (!p2_is_visible)) return {false, cv::Point2i{}, cv::Point2i{}}; - Eigen::Vector3f uv1, uv2; + Eigen::Vector3f uv1; + Eigen::Vector3f uv2; if (p1_is_visible) uv1 = from_camera1 / from_camera1.z(); if (p2_is_visible) uv2 = from_camera2 / from_camera2.z(); if ((p1_is_visible) && (p2_is_visible)) - return {true, cv::Point2i(uv1.x(), uv1.y()), cv::Point2i(uv2.x(), uv2.y())}; + return { + true, cv::Point2i(static_cast(uv1.x()), static_cast(uv1.y())), + cv::Point2i(static_cast(uv2.x()), static_cast(uv2.y()))}; Eigen::Vector3f tangent = from_camera2 - from_camera1; - float mu = (EPSILON - from_camera1.z()) / (tangent.z()); + float mu = (epsilon - from_camera1.z()) / (tangent.z()); if (!p1_is_visible) { from_camera1 = from_camera1 + mu * tangent; uv1 = from_camera1 / from_camera1.z(); @@ -173,11 +174,13 @@ void Lanelet2Overlay::draw_overlay_line_segments( from_camera2 = from_camera1 + mu * tangent; uv2 = from_camera2 / from_camera2.z(); } - return {true, cv::Point2i(uv1.x(), uv1.y()), cv::Point2i(uv2.x(), uv2.y())}; + return { + true, cv::Point2i(static_cast(uv1.x()), static_cast(uv1.y())), + cv::Point2i(static_cast(uv2.x()), static_cast(uv2.y()))}; }; for (const pcl::PointNormal & pn : near_segments) { - auto [success, u1, u2] = projectLineSegment(pn.getVector3fMap(), pn.getNormalVector3fMap()); + auto [success, u1, u2] = project_line_segment(pn.getVector3fMap(), pn.getNormalVector3fMap()); if (success) cv::line(image, u1, u2, cv::Scalar(0, 255, 255), 2); } } @@ -197,7 +200,8 @@ void Lanelet2Overlay::make_vis_marker( marker.color.a = 0.7f; for (const auto pn : ls) { - geometry_msgs::msg::Point p1, p2; + geometry_msgs::msg::Point p1; + geometry_msgs::msg::Point p2; p1.x = pn.x; p1.y = pn.y; p1.z = pn.z; diff --git a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp index c613642628499..9854fa9ab7106 100644 --- a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp @@ -67,7 +67,8 @@ void LineSegmentDetector::execute(const cv::Mat & image, const rclcpp::Time & st std::vector filtered_lines = remove_too_outer_elements(lines, image.size()); for (const cv::Mat & xy_xy : filtered_lines) { - Eigen::Vector3f xy1, xy2; + Eigen::Vector3f xy1; + Eigen::Vector3f xy2; xy1 << xy_xy.at(0), xy_xy.at(1), 0; xy2 << xy_xy.at(2), xy_xy.at(3), 0; pcl::PointNormal pn; @@ -79,7 +80,7 @@ void LineSegmentDetector::execute(const cv::Mat & image, const rclcpp::Time & st } std::vector LineSegmentDetector::remove_too_outer_elements( - const cv::Mat & lines, const cv::Size & size) const + const cv::Mat & lines, const cv::Size & size) { std::vector rect_vector; rect_vector.emplace_back(0, 0, size.width, 3); diff --git a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp index 7e06de81fbd18..70e1bf2d6e6e2 100644 --- a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp @@ -30,11 +30,11 @@ LineSegmentsOverlay::LineSegmentsOverlay(const rclcpp::NodeOptions & options) using std::placeholders::_1; auto cb_image = std::bind(&LineSegmentsOverlay::on_image, this, _1); - auto cb_line_segments_ = std::bind(&LineSegmentsOverlay::on_line_segments, this, _1); + auto cb_line_segments = std::bind(&LineSegmentsOverlay::on_line_segments, this, _1); sub_image_ = create_subscription("~/input/image_raw", 10, cb_image); sub_line_segments_ = - create_subscription("~/input/line_segments", 10, cb_line_segments_); + create_subscription("~/input/line_segments", 10, cb_line_segments); pub_debug_image_ = create_publisher("~/debug/image_with_colored_line_segments", 10); } @@ -73,8 +73,7 @@ void LineSegmentsOverlay::on_line_segments(const PointCloud2::ConstSharedPtr & l LineSegments line_segments_cloud; pcl::fromROSMsg(*line_segments_msg, line_segments_cloud); - for (size_t index = 0; index < line_segments_cloud.size(); ++index) { - const LineSegment & pn = line_segments_cloud.at(index); + for (auto & pn : line_segments_cloud) { Eigen::Vector3f xy1 = pn.getVector3fMap(); Eigen::Vector3f xy2 = pn.getNormalVector3fMap(); @@ -83,7 +82,9 @@ void LineSegmentsOverlay::on_line_segments(const PointCloud2::ConstSharedPtr & l color = cv::Scalar(0, 0, 255); // Red } - cv::line(image, cv::Point(xy1(0), xy1(1)), cv::Point(xy2(0), xy2(1)), color, 2); + cv::line( + image, cv::Point(static_cast(xy1(0)), static_cast(xy1(1))), + cv::Point(static_cast(xy2(0)), static_cast(xy2(1))), color, 2); } common::publish_image(*pub_debug_image_, image, stamp); diff --git a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp index df0aa7d65c617..b844fb69285b5 100644 --- a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp @@ -25,11 +25,11 @@ namespace yabloc::segment_filter { SegmentFilter::SegmentFilter(const rclcpp::NodeOptions & options) : Node("segment_filter", options), - image_size_(declare_parameter("image_size")), - max_range_(declare_parameter("max_range")), - min_segment_length_(declare_parameter("min_segment_length")), - max_segment_distance_(declare_parameter("max_segment_distance")), - max_lateral_distance_(declare_parameter("max_lateral_distance")), + image_size_(static_cast(declare_parameter("image_size"))), + max_range_(static_cast(declare_parameter("max_range"))), + min_segment_length_(static_cast(declare_parameter("min_segment_length"))), + max_segment_distance_(static_cast(declare_parameter("max_segment_distance"))), + max_lateral_distance_(static_cast(declare_parameter("max_lateral_distance"))), info_(this), synchro_subscriber_(this, "~/input/line_segments_cloud", "~/input/graph_segmented"), tf_subscriber_(this->get_clock()) @@ -47,9 +47,12 @@ SegmentFilter::SegmentFilter(const rclcpp::NodeOptions & options) cv::Point2i SegmentFilter::to_cv_point(const Eigen::Vector3f & v) const { - cv::Point pt; - pt.x = -v.y() / max_range_ * image_size_ * 0.5f + image_size_ / 2; - pt.y = -v.x() / max_range_ * image_size_ * 0.5f + image_size_; + cv::Point2i pt; + pt.x = static_cast( + -v.y() / max_range_ * static_cast(image_size_) * 0.5f + + static_cast(image_size_) / 2.0); + pt.y = static_cast( + -v.x() / max_range_ * static_cast(image_size_) * 0.5f + static_cast(image_size_)); return pt; } @@ -149,7 +152,7 @@ void SegmentFilter::execute(const PointCloud2 & line_segments_msg, const Image & pcl::PointXYZLNormal pln; pln.getVector3fMap() = pn.getVector3fMap(); pln.getNormalVector3fMap() = pn.getNormalVector3fMap(); - if (indices.count(index) > 0) + if (indices.count(static_cast(index)) > 0) pln.label = 255; else pln.label = 0; @@ -194,7 +197,7 @@ std::set get_unique_pixel_value(cv::Mat & image) auto last = std::unique(begin, image.end()); std::sort(begin, last); last = std::unique(begin, last); - return std::set(begin, last); + return {begin, last}; } pcl::PointCloud SegmentFilter::project_lines( @@ -204,9 +207,9 @@ pcl::PointCloud SegmentFilter::project_lines( pcl::PointCloud projected_points; for (size_t index = 0; index < points.size(); ++index) { if (negative) { - if (indices.count(index) > 0) continue; + if (indices.count(static_cast(index)) > 0) continue; } else { - if (indices.count(index) == 0) continue; + if (indices.count(static_cast(index)) == 0) continue; } pcl::PointNormal truncated_pn = points.at(index); @@ -254,9 +257,10 @@ std::set SegmentFilter::filter_by_mask( auto & pn = edges.at(i); Eigen::Vector3f p1 = pn.getVector3fMap(); Eigen::Vector3f p2 = pn.getNormalVector3fMap(); - cv::Scalar color = cv::Scalar::all(i + 1); + cv::Scalar color = cv::Scalar::all(static_cast(i + 1)); cv::line( - line_image, cv::Point2i(p1.x(), p1.y()), cv::Point2i(p2.x(), p2.y()), color, 1, + line_image, cv::Point2i(static_cast(p1.x()), static_cast(p1.y())), + cv::Point2i(static_cast(p2.x()), static_cast(p2.y())), color, 1, cv::LineTypes::LINE_4); } @@ -275,7 +279,7 @@ std::set SegmentFilter::filter_by_mask( // Extract edges within masks std::set reliable_indices; for (size_t i = 0; i < edges.size(); i++) { - if (pixel_values.count(i + 1) != 0) reliable_indices.insert(i); + if (pixel_values.count(i + 1) != 0) reliable_indices.insert(static_cast(i)); } return reliable_indices; diff --git a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp index 0714b6c8091c8..c4b868289eb16 100644 --- a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp +++ b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp @@ -39,8 +39,8 @@ class UndistortNode : public rclcpp::Node explicit UndistortNode(const rclcpp::NodeOptions & options) : Node("undistort", options), - OUTPUT_WIDTH(declare_parameter("width")), - OVERRIDE_FRAME_ID(declare_parameter("override_frame_id")) + OUTPUT_WIDTH_(static_cast(declare_parameter("width"))), + OVERRIDE_FRAME_ID_(declare_parameter("override_frame_id")) { using std::placeholders::_1; @@ -63,8 +63,8 @@ class UndistortNode : public rclcpp::Node } private: - const int OUTPUT_WIDTH; - const std::string OVERRIDE_FRAME_ID; + const int OUTPUT_WIDTH_; + const std::string OVERRIDE_FRAME_ID_; rclcpp::Subscription::SharedPtr sub_image_; rclcpp::Subscription::SharedPtr sub_compressed_image_; @@ -74,29 +74,33 @@ class UndistortNode : public rclcpp::Node std::optional info_{std::nullopt}; std::optional scaled_info_{std::nullopt}; - cv::Mat undistort_map_x, undistort_map_y; + cv::Mat undistort_map_x_, undistort_map_y_; void make_remap_lut() { if (!info_.has_value()) return; - cv::Mat K = cv::Mat(cv::Size(3, 3), CV_64FC1, reinterpret_cast(info_->k.data())); - cv::Mat D = cv::Mat(cv::Size(5, 1), CV_64FC1, reinterpret_cast(info_->d.data())); - cv::Size size(info_->width, info_->height); + cv::Mat k = cv::Mat(cv::Size(3, 3), CV_64FC1, info_->k.data()); + cv::Mat d = cv::Mat(cv::Size(5, 1), CV_64FC1, info_->d.data()); + cv::Size size(static_cast(info_->width), static_cast(info_->height)); cv::Size new_size = size; - if (OUTPUT_WIDTH > 0) - new_size = cv::Size(OUTPUT_WIDTH, 1.0f * OUTPUT_WIDTH / size.width * size.height); + if (OUTPUT_WIDTH_ > 0) + // set new_size along with aspect ratio + new_size = cv::Size( + OUTPUT_WIDTH_, static_cast( + static_cast(OUTPUT_WIDTH_) * + (static_cast(size.height) / static_cast(size.width)))); - cv::Mat new_K = cv::getOptimalNewCameraMatrix(K, D, size, 0, new_size); + cv::Mat new_k = cv::getOptimalNewCameraMatrix(k, d, size, 0, new_size); cv::initUndistortRectifyMap( - K, D, cv::Mat(), new_K, new_size, CV_32FC1, undistort_map_x, undistort_map_y); + k, d, cv::Mat(), new_k, new_size, CV_32FC1, undistort_map_x_, undistort_map_y_); scaled_info_ = sensor_msgs::msg::CameraInfo{}; - scaled_info_->k.at(0) = new_K.at(0, 0); - scaled_info_->k.at(2) = new_K.at(0, 2); - scaled_info_->k.at(4) = new_K.at(1, 1); - scaled_info_->k.at(5) = new_K.at(1, 2); + scaled_info_->k.at(0) = new_k.at(0, 0); + scaled_info_->k.at(2) = new_k.at(0, 2); + scaled_info_->k.at(4) = new_k.at(1, 1); + scaled_info_->k.at(5) = new_k.at(1, 2); scaled_info_->k.at(8) = 1; scaled_info_->d.resize(5); scaled_info_->width = new_size.width; @@ -106,12 +110,12 @@ class UndistortNode : public rclcpp::Node void remap_and_publish(const cv::Mat & image, const std_msgs::msg::Header & header) { cv::Mat undistorted_image; - cv::remap(image, undistorted_image, undistort_map_x, undistort_map_y, cv::INTER_LINEAR); + cv::remap(image, undistorted_image, undistort_map_x_, undistort_map_y_, cv::INTER_LINEAR); // Publish CameraInfo { scaled_info_->header = info_->header; - if (OVERRIDE_FRAME_ID != "") scaled_info_->header.frame_id = OVERRIDE_FRAME_ID; + if (!OVERRIDE_FRAME_ID_.empty()) scaled_info_->header.frame_id = OVERRIDE_FRAME_ID_; pub_info_->publish(scaled_info_.value()); } @@ -119,8 +123,8 @@ class UndistortNode : public rclcpp::Node { cv_bridge::CvImage bridge; bridge.header.stamp = header.stamp; - if (OVERRIDE_FRAME_ID != "") - bridge.header.frame_id = OVERRIDE_FRAME_ID; + if (!OVERRIDE_FRAME_ID_.empty()) + bridge.header.frame_id = OVERRIDE_FRAME_ID_; else bridge.header.frame_id = header.frame_id; bridge.encoding = "bgr8"; @@ -134,7 +138,7 @@ class UndistortNode : public rclcpp::Node if (!info_.has_value()) { return; } - if (undistort_map_x.empty()) { + if (undistort_map_x_.empty()) { make_remap_lut(); } @@ -153,7 +157,7 @@ class UndistortNode : public rclcpp::Node if (!info_.has_value()) { return; } - if (undistort_map_x.empty()) { + if (undistort_map_x_.empty()) { make_remap_lut(); } diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 383051e8f67a5..e5b95b613b0d1 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -33,13 +33,9 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi msg.vertical_datum = data["vertical_datum"].as(); msg.mgrs_grid = data["mgrs_grid"].as(); - } else if (msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM) { - msg.vertical_datum = data["vertical_datum"].as(); - msg.map_origin.latitude = data["map_origin"]["latitude"].as(); - msg.map_origin.longitude = data["map_origin"]["longitude"].as(); - msg.map_origin.altitude = data["map_origin"]["altitude"].as(); - - } else if (msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::TRANSVERSE_MERCATOR) { + } else if ( + msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM || + msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::TRANSVERSE_MERCATOR) { msg.vertical_datum = data["vertical_datum"].as(); msg.map_origin.latitude = data["map_origin"]["latitude"].as(); msg.map_origin.longitude = data["map_origin"]["longitude"].as(); diff --git a/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp b/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp index fdfe96434fe0d..7d66388c1a91e 100644 --- a/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp +++ b/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp @@ -41,7 +41,7 @@ void save_dummy_mgrs_lanelet2_map(const std::string & mgrs_coord, const std::str file << "\n"; file << "\n"; - file << " \n"; + file << R"( \n"; file << ""; file.close();