diff --git a/.github/dependabot.yaml b/.github/dependabot.yaml index 9a2fbd7542dad..3f3bf243f639d 100644 --- a/.github/dependabot.yaml +++ b/.github/dependabot.yaml @@ -5,3 +5,6 @@ updates: schedule: interval: daily open-pull-requests-limit: 1 + labels: + - bot + - github-actions diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml index e1b72f706881c..7fd1cc90fe97d 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.github/workflows/pre-commit.yaml @@ -5,12 +5,28 @@ on: jobs: pre-commit: + if: ${{ github.event.repository.private }} runs-on: ubuntu-latest steps: + - name: Generate token + id: generate-token + uses: tibdex/github-app-token@v1 + with: + app_id: ${{ secrets.APP_ID }} + private_key: ${{ secrets.PRIVATE_KEY }} + - name: Check out repository uses: actions/checkout@v3 + with: + ref: ${{ github.event.pull_request.head.ref }} + + - name: Set git config + uses: autowarefoundation/autoware-github-actions/set-git-config@v1 + with: + token: ${{ steps.generate-token.outputs.token }} - name: Run pre-commit uses: autowarefoundation/autoware-github-actions/pre-commit@v1 with: pre-commit-config: .pre-commit-config.yaml + token: ${{ steps.generate-token.outputs.token }} diff --git a/.github/workflows/sync-files.yaml b/.github/workflows/sync-files.yaml index 32f4613f6de7a..69ef19c684c69 100644 --- a/.github/workflows/sync-files.yaml +++ b/.github/workflows/sync-files.yaml @@ -20,3 +20,7 @@ jobs: uses: autowarefoundation/autoware-github-actions/sync-files@v1 with: token: ${{ steps.generate-token.outputs.token }} + pr-labels: | + bot + sync-files + auto-merge-method: squash diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index fa2480a48ee2e..733a842865802 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -4,7 +4,7 @@ ci: repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.1.0 + rev: v4.2.0 hooks: - id: check-json - id: check-merge-conflict diff --git a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp index 351a804d01c8a..61924dc8802a6 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp @@ -114,12 +114,12 @@ void InteractiveObject::update(const Ogre::Vector3 & point) : std::atan2(velocity_.y, velocity_.x); } +void InteractiveObject::reset() { velocity_ = Ogre::Vector3::ZERO; } + double InteractiveObject::distance(const Ogre::Vector3 & point) { return point_.distance(point); } InteractiveObjectCollection::InteractiveObjectCollection() { target_ = nullptr; } -void InteractiveObjectCollection::reset() { target_ = nullptr; } - void InteractiveObjectCollection::select(const Ogre::Vector3 & point) { const size_t index = nearest(point); @@ -128,6 +128,19 @@ void InteractiveObjectCollection::select(const Ogre::Vector3 & point) } } +boost::optional> InteractiveObjectCollection::reset() +{ + if (!target_) { + return {}; + } + + const auto uuid = target_->uuid(); + target_->reset(); + target_ = nullptr; + + return uuid; +} + boost::optional> InteractiveObjectCollection::create( const Ogre::Vector3 & point) { @@ -304,7 +317,8 @@ int InteractiveObjectTool::processMouseEvent(rviz_common::ViewportMouseEvent & e } if (event.rightUp()) { - objects_.reset(); + const auto uuid = objects_.reset(); + publishObjectMsg(uuid.get(), Object::MODIFY); return 0; } diff --git a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp index d33a7ea8b4e2c..6d2a3b30c5ea9 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp @@ -90,6 +90,7 @@ class InteractiveObject void twist(geometry_msgs::msg::Twist & twist) const; void transform(tf2::Transform & tf_map2object) const; void update(const Ogre::Vector3 & point); + void reset(); double distance(const Ogre::Vector3 & point); private: @@ -105,8 +106,8 @@ class InteractiveObjectCollection InteractiveObjectCollection(); ~InteractiveObjectCollection() {} - void reset(); void select(const Ogre::Vector3 & point); + boost::optional> reset(); boost::optional> create(const Ogre::Vector3 & point); boost::optional> remove(const Ogre::Vector3 & point); boost::optional> update(const Ogre::Vector3 & point); diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp index cdb735f76845f..3ffc1aef30744 100644 --- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp +++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp @@ -43,11 +43,25 @@ TrafficLightPublishPanel::TrafficLightPublishPanel(QWidget * parent) : rviz_comm traffic_light_id_input_->setRange(0, 999999); traffic_light_id_input_->setValue(0); + // Traffic Light Confidence + traffic_light_confidence_input_ = new QDoubleSpinBox(); + traffic_light_confidence_input_->setRange(0.0, 1.0); + traffic_light_confidence_input_->setSingleStep(0.1); + traffic_light_confidence_input_->setValue(1.0); + // Traffic Light Color light_color_combo_ = new QComboBox(); - light_color_combo_->addItems( - {"RED", "AMBER", "GREEN", "WHITE", "LEFT_ARROW", "RIGHT_ARROW", "UP_ARROW", "DOWN_ARROW", - "DOWN_LEFT_ARROW", "DOWN_RIGHT_ARROW", "FLASHING", "UNKNOWN"}); + light_color_combo_->addItems({"RED", "AMBER", "GREEN", "WHITE", "UNKNOWN"}); + + // Traffic Light Shape + light_shape_combo_ = new QComboBox(); + light_shape_combo_->addItems( + {"CIRCLE", "LEFT_ARROW", "RIGHT_ARROW", "UP_ARROW", "DOWN_ARROW", "DOWN_LEFT_ARROW", + "DOWN_RIGHT_ARROW", "CROSS", "UNKNOWN"}); + + // Traffic Light Status + light_status_combo_ = new QComboBox(); + light_status_combo_->addItems({"SOLID_ON", "SOLID_OFF", "FLASHING", "UNKNOWN"}); // Set Traffic Signals Button set_button_ = new QPushButton("SET"); @@ -64,8 +78,8 @@ TrafficLightPublishPanel::TrafficLightPublishPanel(QWidget * parent) : rviz_comm horizontal_header->setSectionResizeMode(QHeaderView::Stretch); traffic_table_ = new QTableWidget(); - traffic_table_->setColumnCount(2); - traffic_table_->setHorizontalHeaderLabels({"ID", "Status"}); + traffic_table_->setColumnCount(5); + traffic_table_->setHorizontalHeaderLabels({"ID", "Color", "Shape", "Status", "Confidence"}); traffic_table_->setVerticalHeader(vertical_header); traffic_table_->setHorizontalHeader(horizontal_header); @@ -77,34 +91,48 @@ TrafficLightPublishPanel::TrafficLightPublishPanel(QWidget * parent) : rviz_comm auto * h_layout_1 = new QHBoxLayout; h_layout_1->addWidget(new QLabel("Rate: ")); h_layout_1->addWidget(publishing_rate_input_); - h_layout_1->addWidget(new QLabel("Traffic Light ID: ")); + h_layout_1->addWidget(new QLabel("ID: ")); h_layout_1->addWidget(traffic_light_id_input_); + h_layout_1->addWidget(new QLabel("Confidence: ")); + h_layout_1->addWidget(traffic_light_confidence_input_); auto * h_layout_2 = new QHBoxLayout; - h_layout_2->addWidget(new QLabel("Traffic Light Status: ")); - h_layout_2->addWidget(light_color_combo_); + h_layout_2->addWidget(new QLabel("Traffic Light Color: "), 40); + h_layout_2->addWidget(light_color_combo_, 60); + + auto * h_layout_3 = new QHBoxLayout; + h_layout_3->addWidget(new QLabel("Traffic Light Shape: "), 40); + h_layout_3->addWidget(light_shape_combo_, 60); + + auto * h_layout_4 = new QHBoxLayout; + h_layout_4->addWidget(new QLabel("Traffic Light Status: "), 40); + h_layout_4->addWidget(light_status_combo_, 60); auto * v_layout = new QVBoxLayout; v_layout->addLayout(h_layout_1); v_layout->addLayout(h_layout_2); + v_layout->addLayout(h_layout_3); + v_layout->addLayout(h_layout_4); v_layout->addWidget(set_button_); v_layout->addWidget(reset_button_); v_layout->addWidget(publish_button_); - auto * h_layout_3 = new QHBoxLayout; - h_layout_3->addLayout(v_layout); - h_layout_3->addWidget(traffic_table_); + auto * h_layout_5 = new QHBoxLayout; + h_layout_5->addLayout(v_layout); + h_layout_5->addWidget(traffic_table_); - setLayout(h_layout_3); + setLayout(h_layout_5); } void TrafficLightPublishPanel::onSetTrafficLightState() { const auto traffic_light_id = traffic_light_id_input_->value(); const auto color = light_color_combo_->currentText(); + const auto shape = light_shape_combo_->currentText(); + const auto status = light_status_combo_->currentText(); TrafficLight traffic_light; - traffic_light.confidence = 1.0; + traffic_light.confidence = traffic_light_confidence_input_->value(); if (color == "RED") { traffic_light.color = TrafficLight::RED; @@ -114,24 +142,38 @@ void TrafficLightPublishPanel::onSetTrafficLightState() traffic_light.color = TrafficLight::GREEN; } else if (color == "WHITE") { traffic_light.color = TrafficLight::WHITE; - } else if (color == "LEFT_ARROW") { - traffic_light.color = TrafficLight::LEFT_ARROW; - } else if (color == "RIGHT_ARROW") { - traffic_light.color = TrafficLight::RIGHT_ARROW; - } else if (color == "UP_ARROW") { - traffic_light.color = TrafficLight::UP_ARROW; - } else if (color == "DOWN_ARROW") { - traffic_light.color = TrafficLight::DOWN_ARROW; - } else if (color == "DOWN_LEFT_ARROW") { - traffic_light.color = TrafficLight::DOWN_LEFT_ARROW; - } else if (color == "DOWN_RIGHT_ARROW") { - traffic_light.color = TrafficLight::DOWN_RIGHT_ARROW; - } else if (color == "FLASHING") { - traffic_light.color = TrafficLight::FLASHING; } else if (color == "UNKNOWN") { traffic_light.color = TrafficLight::UNKNOWN; } + if (shape == "CIRCLE") { + traffic_light.shape = TrafficLight::CIRCLE; + } else if (shape == "LEFT_ARROW") { + traffic_light.shape = TrafficLight::LEFT_ARROW; + } else if (shape == "RIGHT_ARROW") { + traffic_light.shape = TrafficLight::RIGHT_ARROW; + } else if (shape == "UP_ARROW") { + traffic_light.shape = TrafficLight::UP_ARROW; + } else if (shape == "DOWN_ARROW") { + traffic_light.shape = TrafficLight::DOWN_ARROW; + } else if (shape == "DOWN_LEFT_ARROW") { + traffic_light.shape = TrafficLight::DOWN_LEFT_ARROW; + } else if (shape == "DOWN_RIGHT_ARROW") { + traffic_light.shape = TrafficLight::DOWN_RIGHT_ARROW; + } else if (shape == "UNKNOWN") { + traffic_light.shape = TrafficLight::UNKNOWN; + } + + if (status == "SOLID_OFF") { + traffic_light.status = TrafficLight::SOLID_OFF; + } else if (status == "SOLID_ON") { + traffic_light.status = TrafficLight::SOLID_ON; + } else if (status == "FLASHING") { + traffic_light.status = TrafficLight::FLASHING; + } else if (status == "UNKNOWN") { + traffic_light.status = TrafficLight::UNKNOWN; + } + TrafficSignal traffic_signal; traffic_signal.lights.push_back(traffic_light); traffic_signal.map_primitive_id = traffic_light_id; @@ -234,44 +276,77 @@ void TrafficLightPublishPanel::onTimer() color_label->setText("WHITE"); color_label->setStyleSheet("background-color: #FFFFFF;"); break; + case TrafficLight::UNKNOWN: + color_label->setText("UNKNOWN"); + color_label->setStyleSheet("background-color: #808080;"); + break; + default: + break; + } + + auto shape_label = new QLabel(); + shape_label->setAlignment(Qt::AlignCenter); + + switch (light.shape) { + case TrafficLight::CIRCLE: + shape_label->setText("CIRCLE"); + break; case TrafficLight::LEFT_ARROW: - color_label->setText("LEFT_ARROW"); - color_label->setStyleSheet("background-color: #7CFC00;"); + shape_label->setText("LEFT_ARROW"); break; case TrafficLight::RIGHT_ARROW: - color_label->setText("RIGHT_ARROW"); - color_label->setStyleSheet("background-color: #7CFC00;"); + shape_label->setText("RIGHT_ARROW"); break; case TrafficLight::UP_ARROW: - color_label->setText("UP_ARROW"); - color_label->setStyleSheet("background-color: #7CFC00;"); + shape_label->setText("UP_ARROW"); break; case TrafficLight::DOWN_ARROW: - color_label->setText("DOWN_ARROW"); - color_label->setStyleSheet("background-color: #7CFC00;"); + shape_label->setText("DOWN_ARROW"); break; case TrafficLight::DOWN_LEFT_ARROW: - color_label->setText("DOWN_LEFT_ARROW"); - color_label->setStyleSheet("background-color: #7CFC00;"); + shape_label->setText("DOWN_LEFT_ARROW"); break; case TrafficLight::DOWN_RIGHT_ARROW: - color_label->setText("DOWN_RIGHT_ARROW"); - color_label->setStyleSheet("background-color: #7CFC00;"); + shape_label->setText("DOWN_RIGHT_ARROW"); break; case TrafficLight::FLASHING: - color_label->setText("FLASHING"); - color_label->setStyleSheet("background-color: #7CFC00;"); + shape_label->setText("FLASHING"); break; case TrafficLight::UNKNOWN: - color_label->setText("UNKNOWN"); - color_label->setStyleSheet("background-color: #808080;"); + shape_label->setText("UNKNOWN"); break; default: break; } + auto status_label = new QLabel(); + status_label->setAlignment(Qt::AlignCenter); + + switch (light.status) { + case TrafficLight::SOLID_OFF: + status_label->setText("SOLID_OFF"); + break; + case TrafficLight::SOLID_ON: + status_label->setText("SOLID_ON"); + break; + case TrafficLight::FLASHING: + status_label->setText("FLASHING"); + break; + case TrafficLight::UNKNOWN: + status_label->setText("UNKNOWN"); + break; + default: + break; + } + + auto confidence_label = new QLabel(QString::number(light.confidence)); + confidence_label->setAlignment(Qt::AlignCenter); + traffic_table_->setCellWidget(i, 0, id_label); traffic_table_->setCellWidget(i, 1, color_label); + traffic_table_->setCellWidget(i, 2, shape_label); + traffic_table_->setCellWidget(i, 3, status_label); + traffic_table_->setCellWidget(i, 4, confidence_label); } } diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp index 7eb81fa2f3ceb..d63f0f7d4d14a 100644 --- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp +++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp @@ -60,7 +60,10 @@ public Q_SLOTS: QSpinBox * publishing_rate_input_; QSpinBox * traffic_light_id_input_; + QDoubleSpinBox * traffic_light_confidence_input_; QComboBox * light_color_combo_; + QComboBox * light_shape_combo_; + QComboBox * light_status_combo_; QPushButton * set_button_; QPushButton * reset_button_; QPushButton * publish_button_; diff --git a/control/joy_controller/README.md b/control/joy_controller/README.md index a494a9e6fb330..4df53e52e3cc1 100644 --- a/control/joy_controller/README.md +++ b/control/joy_controller/README.md @@ -41,3 +41,24 @@ | `max_forward_velocity` | double | absolute max velocity to go forward | | `max_backward_velocity` | double | absolute max velocity to go backward | | `backward_accel_ratio` | double | ratio to calculate deceleration (commanded acceleration is -ratio \* operation) | + +## P65 Joystick Key Map + +| Acceleration | R2 | +| -------------------- | --------------------- | +| Brake | L2 | +| Steering | Left Stick Left Right | +| Shift up | Cursor Up | +| Shift down | Cursor Down | +| Shift Drive | Cursor Left | +| Shift Reverse | Cursor Right | +| Turn Signal Left | L1 | +| Turn Signal Right | R1 | +| Clear Turn Signal | A | +| Gate Mode | B | +| Emergency Stop | Select | +| Clear Emergency Stop | Start | +| Autoware Engage | X | +| Autoware Disengage | Y | +| Vehicle Engage | PS | +| Vehicle Disengage | Right Trigger | diff --git a/control/joy_controller/include/joy_controller/joy_converter/p65_joy_converter.hpp b/control/joy_controller/include/joy_controller/joy_converter/p65_joy_converter.hpp new file mode 100644 index 0000000000000..e4cb822846fef --- /dev/null +++ b/control/joy_controller/include/joy_controller/joy_converter/p65_joy_converter.hpp @@ -0,0 +1,81 @@ +// Copyright 2022 Leo Drive Teknoloji A.Ş., 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 JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ +#define JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ + +#include "joy_controller/joy_converter/joy_converter_base.hpp" + +#include + +namespace joy_controller +{ +class P65JoyConverter : public JoyConverterBase +{ +public: + explicit P65JoyConverter(const sensor_msgs::msg::Joy & j) : j_(j) {} + + float accel() const { return std::max(0.0f, -((R2() - 1.0f) / 2.0f)); } + + float brake() const { return std::max(0.0f, -((L2() - 1.0f) / 2.0f)); } + + float steer() const { return LStickLeftRight(); } + + bool shift_up() const { return CursorUpDown() == 1.0f; } + bool shift_down() const { return CursorUpDown() == -1.0f; } + bool shift_drive() const { return CursorLeftRight() == 1.0f; } + bool shift_reverse() const { return CursorLeftRight() == -1.0f; } + + bool turn_signal_left() const { return L1(); } + bool turn_signal_right() const { return R1(); } + bool clear_turn_signal() const { return A(); } + + bool gate_mode() const { return B(); } + + bool emergency_stop() const { return Select(); } + bool clear_emergency_stop() const { return Start(); } + + bool autoware_engage() const { return X(); } + bool autoware_disengage() const { return Y(); } + + bool vehicle_engage() const { return PS(); } + bool vehicle_disengage() const { return RTrigger(); } + +private: + float LStickLeftRight() const { return j_.axes.at(0); } + float LStickUpDown() const { return j_.axes.at(1); } + float RStickLeftRight() const { return j_.axes.at(3); } + float RStickUpDown() const { return j_.axes.at(4); } + float CursorLeftRight() const { return j_.axes.at(6); } + float CursorUpDown() const { return j_.axes.at(7); } + float L2() const { return j_.axes.at(2); } + float R2() const { return j_.axes.at(5); } + + bool A() const { return j_.buttons.at(0); } + bool B() const { return j_.buttons.at(1); } + bool X() const { return j_.buttons.at(2); } + bool Y() const { return j_.buttons.at(3); } + bool L1() const { return j_.buttons.at(4); } + bool R1() const { return j_.buttons.at(5); } + bool Select() const { return j_.buttons.at(6); } + bool Start() const { return j_.buttons.at(7); } + bool PS() const { return j_.buttons.at(8); } + bool LTrigger() const { return j_.buttons.at(9); } + bool RTrigger() const { return j_.buttons.at(10); } + + const sensor_msgs::msg::Joy j_; +}; +} // namespace joy_controller + +#endif // JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ diff --git a/control/joy_controller/launch/joy_controller.launch.xml b/control/joy_controller/launch/joy_controller.launch.xml index 8642e830ba24a..b2d4035411b0c 100644 --- a/control/joy_controller/launch/joy_controller.launch.xml +++ b/control/joy_controller/launch/joy_controller.launch.xml @@ -1,5 +1,5 @@ - + diff --git a/control/joy_controller/src/joy_controller/joy_controller_node.cpp b/control/joy_controller/src/joy_controller/joy_controller_node.cpp index 5d61be904f845..0e0ebf03fe0ed 100644 --- a/control/joy_controller/src/joy_controller/joy_controller_node.cpp +++ b/control/joy_controller/src/joy_controller/joy_controller_node.cpp @@ -15,6 +15,7 @@ #include "joy_controller/joy_controller.hpp" #include "joy_controller/joy_converter/ds4_joy_converter.hpp" #include "joy_controller/joy_converter/g29_joy_converter.hpp" +#include "joy_controller/joy_converter/p65_joy_converter.hpp" #include @@ -151,8 +152,10 @@ void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPt last_joy_received_time_ = msg->header.stamp; if (joy_type_ == "G29") { joy_ = std::make_shared(*msg); - } else { + } else if (joy_type_ == "DS4") { joy_ = std::make_shared(*msg); + } else { + joy_ = std::make_shared(*msg); } if (joy_->shift_up() || joy_->shift_down() || joy_->shift_drive() || joy_->shift_reverse()) { diff --git a/control/trajectory_follower_nodes/CMakeLists.txt b/control/trajectory_follower_nodes/CMakeLists.txt index 32e4c5d42523b..4ec4da305ca35 100644 --- a/control/trajectory_follower_nodes/CMakeLists.txt +++ b/control/trajectory_follower_nodes/CMakeLists.txt @@ -68,7 +68,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() # Unit tests set(TRAJECTORY_FOLLOWER_NODES_TEST test_trajectory_follower_nodes) - ament_add_gtest(${TRAJECTORY_FOLLOWER_NODES_TEST} + ament_add_ros_isolated_gtest(${TRAJECTORY_FOLLOWER_NODES_TEST} test/trajectory_follower_test_utils.hpp test/test_latlon_muxer_node.cpp test/test_lateral_controller_node.cpp diff --git a/control/trajectory_follower_nodes/package.xml b/control/trajectory_follower_nodes/package.xml index 6f22e6bddff2f..215ebaea5187b 100644 --- a/control/trajectory_follower_nodes/package.xml +++ b/control/trajectory_follower_nodes/package.xml @@ -21,7 +21,7 @@ ros2launch - ament_cmake_gtest + ament_cmake_ros ament_index_python diff --git a/launch/tier4_autoware_api_launch/CMakeLists.txt b/launch/tier4_autoware_api_launch/CMakeLists.txt new file mode 100644 index 0000000000000..95c4157c0122a --- /dev/null +++ b/launch/tier4_autoware_api_launch/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.5) +project(tier4_autoware_api_launch) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/launch/tier4_autoware_api_launch/README.md b/launch/tier4_autoware_api_launch/README.md new file mode 100644 index 0000000000000..e2ea11b7de4c1 --- /dev/null +++ b/launch/tier4_autoware_api_launch/README.md @@ -0,0 +1,24 @@ +# tier4_autoware_api_launch + +## Description + +This package contains launch files that run nodes to convert Autoware internal topics into consistent API used by external software (e.g., fleet management system, simulator). + +## Package Dependencies + +Please see `` in `package.xml`. + +## Usage + +You can include as follows in `*.launch.xml` to use `control.launch.py`. + +```xml + + + + +``` + +## Notes + +For reducing processing load, we use the [Component](https://docs.ros.org/en/galactic/Concepts/About-Composition.html) feature in ROS2 (similar to Nodelet in ROS1 ) diff --git a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml new file mode 100644 index 0000000000000..3d9113f436eca --- /dev/null +++ b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/launch/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py b/launch/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py new file mode 100644 index 0000000000000..4f1295588a834 --- /dev/null +++ b/launch/tier4_autoware_api_launch/launch/include/external_api_adaptor.launch.py @@ -0,0 +1,57 @@ +# Copyright 2021 Tier IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def _create_api_node(node_name, class_name, **kwargs): + return ComposableNode( + namespace="external", + name=node_name, + package="autoware_iv_external_api_adaptor", + plugin="external_api::" + class_name, + **kwargs + ) + + +def generate_launch_description(): + components = [ + _create_api_node("cpu_usage", "CpuUsage"), + _create_api_node("diagnostics", "Diagnostics"), + _create_api_node("door", "Door"), + _create_api_node("emergency", "Emergency"), + _create_api_node("engage", "Engage"), + _create_api_node("fail_safe_state", "FailSafeState"), + _create_api_node("initial_pose", "InitialPose"), + _create_api_node("map", "Map"), + _create_api_node("operator", "Operator"), + _create_api_node("metadata_packages", "MetadataPackages"), + _create_api_node("route", "Route"), + _create_api_node("service", "Service"), + _create_api_node("start", "Start"), + _create_api_node("vehicle_status", "VehicleStatus"), + _create_api_node("velocity", "Velocity"), + _create_api_node("version", "Version"), + ] + container = ComposableNodeContainer( + namespace="external", + name="autoware_iv_adaptor", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=components, + output="screen", + ) + return launch.LaunchDescription([container]) diff --git a/launch/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py b/launch/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py new file mode 100644 index 0000000000000..4b8513f544588 --- /dev/null +++ b/launch/tier4_autoware_api_launch/launch/include/internal_api_adaptor.launch.py @@ -0,0 +1,51 @@ +# Copyright 2021 Tier IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def _create_api_node(node_name, class_name, **kwargs): + return ComposableNode( + namespace="internal", + name=node_name, + package="autoware_iv_internal_api_adaptor", + plugin="internal_api::" + class_name, + **kwargs + ) + + +def generate_launch_description(): + param_initial_pose = { + "init_simulator_pose": LaunchConfiguration("init_simulator_pose"), + "init_localization_pose": LaunchConfiguration("init_localization_pose"), + } + components = [ + _create_api_node("initial_pose", "InitialPose", parameters=[param_initial_pose]), + _create_api_node("iv_msgs", "IVMsgs"), + _create_api_node("operator", "Operator"), + _create_api_node("route", "Route"), + _create_api_node("velocity", "Velocity"), + ] + container = ComposableNodeContainer( + namespace="internal", + name="autoware_iv_adaptor", + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=components, + output="screen", + ) + return launch.LaunchDescription([container]) diff --git a/launch/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml b/launch/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml new file mode 100644 index 0000000000000..810e6a566bd3a --- /dev/null +++ b/launch/tier4_autoware_api_launch/launch/include/internal_api_relay.launch.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_autoware_api_launch/package.xml b/launch/tier4_autoware_api_launch/package.xml new file mode 100644 index 0000000000000..a455e71ffd0bb --- /dev/null +++ b/launch/tier4_autoware_api_launch/package.xml @@ -0,0 +1,25 @@ + + + + tier4_autoware_api_launch + 0.0.0 + The tier4_autoware_api_launch package + Takagi, Isamu + Ryohsuke, Mitsudome + Apache License 2.0 + + ament_cmake_auto + + autoware_iv_external_api_adaptor + autoware_iv_internal_api_adaptor + awapi_awiv_adapter + path_distance_calculator + topic_tools + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml index 88736ec29fa3a..f8a129af4d2a4 100644 --- a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml +++ b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml @@ -44,8 +44,8 @@ # -- vehicle model -- vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics input_delay: 0.24 # steering input delay time for delay compensation - vehicle_model_steer_tau: 0.1 # steering dynamics time constant (1d approximation) [s] - steer_lim_deg: 20.0 # steering angle limit [deg] + vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s] + steer_lim_deg: 40.0 # steering angle limit [deg] steer_rate_lim_dps: 600.0 # steering angle rate limit [deg/s] acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss] velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s] diff --git a/launch/tier4_localization_launch/launch/twist_estimator/twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/twist_estimator/twist_estimator.launch.xml index f4887390c5a05..010010ffcb8bb 100644 --- a/launch/tier4_localization_launch/launch/twist_estimator/twist_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/twist_estimator/twist_estimator.launch.xml @@ -7,5 +7,6 @@ + diff --git a/launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml b/launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml index 33addb108dded..e3bef09e52b47 100644 --- a/launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml +++ b/launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml @@ -19,5 +19,5 @@ global_slope_max_angle_deg: 10.0 local_slope_max_angle_deg: 30.0 split_points_distance_tolerance: 0.2 - split_height_distance: 0.2 - use_virtual_ground_point: False + split_height_distance: 0.3 + use_virtual_ground_point: True diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 3916676fec64e..a288210b06374 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -54,7 +54,7 @@ - + @@ -81,7 +81,7 @@ - + diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index 0db61008d6fdc..d8ba846e24613 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -13,6 +13,7 @@ compare_map_segmentation euclidean_cluster ground_segmentation + image_projection_based_fusion image_transport_decompressor lidar_apollo_instance_segmentation map_based_prediction @@ -22,7 +23,6 @@ occupancy_grid_map_outlier_filter pointcloud_preprocessor pointcloud_to_laserscan - roi_cluster_fusion shape_estimation tensorrt_yolo traffic_light_classifier diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml new file mode 100644 index 0000000000000..31f75d7f7c5df --- /dev/null +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + blind_spot: + use_pass_judge_line: true + stop_line_margin: 1.0 # [m] + backward_length: 15.0 # [m] + ignore_width_from_center_line: 0.7 # [m] + max_future_movement_time: 10.0 # [second] diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml new file mode 100644 index 0000000000000..92e8ba6801382 --- /dev/null +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + crosswalk: + crosswalk: + stop_line_distance: 1.5 # make stop line away from crosswalk when no explicit stop line exists + stop_margin: 1.0 + slow_margin: 2.0 + slow_velocity: 2.76 # 2.76 m/s = 10.0 kmph + stop_predicted_object_prediction_time_margin: 3.0 + + walkway: + stop_line_distance: 1.5 # make stop line away from walkway when no explicit stop line exists + stop_margin: 1.0 + stop_duration_sec: 1.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml new file mode 100644 index 0000000000000..9174045bf0150 --- /dev/null +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + detection_area: + stop_margin: 0.0 + use_dead_line: false + dead_line_margin: 5.0 + use_pass_judge_line: false + state_clear_time: 2.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml new file mode 100644 index 0000000000000..1725a4d0193db --- /dev/null +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -0,0 +1,19 @@ +/**: + ros__parameters: + intersection: + state_transit_margin_time: 0.5 + decel_velocity: 8.33 # 8.33m/s = 30.0km/h + stop_line_margin: 3.0 + stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. + stuck_vehicle_ignore_dist: 7.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) + stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h + intersection_velocity: 2.778 # 2.778m/s = 10.0km/h + intersection_max_accel: 0.5 # m/ss + detection_area_margin: 0.5 # [m] + detection_area_length: 200.0 # [m] + min_predicted_path_confidence: 0.05 + collision_start_margin_time: 5.0 # [s] + collision_end_margin_time: 2.0 # [s] + + merge_from_private_road: + stop_duration_sec: 1.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_stopping_area.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_stopping_area.param.yaml new file mode 100644 index 0000000000000..32cd05a9cc153 --- /dev/null +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_stopping_area.param.yaml @@ -0,0 +1,10 @@ +/**: + ros__parameters: + no_stopping_area: + state_clear_time: 2.0 # [s] time to clear stop state + stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h + stop_margin: 0.0 # [m] margin to stop line at no stopping area + dead_line_margin: 1.0 # [m] dead line offset go at no stopping area + stop_line_margin: 1.0 # [m] margin to auto-gen stop line at no stopping area + detection_area_length: 200.0 # [m] used to create detection area polygon + stuck_vehicle_front_margin: 6.0 # [m] obstacle stop max distance(5.0m) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml new file mode 100644 index 0000000000000..a2f16ee825927 --- /dev/null +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml @@ -0,0 +1,34 @@ +/**: + ros__parameters: + occlusion_spot: + detection_method: "occupancy_grid" # [-] candidate is "occupancy_grid" or "predicted_object" + pass_judge: "smooth_velocity" # [-] candidate is "smooth_velocity" or "current_velocity" + filter_occupancy_grid: true # [-] whether to filter occupancy grid by morphologyEx or not + use_object_info: true # [-] whether to reflect object info to occupancy grid map or not + use_partition_lanelet: true # [-] whether to use partition lanelet map data + pedestrian_vel: 1.5 # [m/s] assume pedestrian is dashing from occlusion at this velocity + pedestrian_radius: 0.3 # [m] assume pedestrian width(0.2m) + margin(0.1m) + debug: # !Note: default should be false for performance + is_show_occlusion: false # [-] whether to show occlusion point markers. + is_show_cv_window: false # [-] whether to show open_cv debug window + is_show_processing_time: false # [-] whether to show processing time + threshold: + detection_area_length: 100.0 # [m] the length of path to consider perception range + stuck_vehicle_vel: 1.0 # [m/s] velocity below this value is assumed to stop + lateral_distance: 1.5 # [m] maximum lateral distance to consider hidden collision + motion: + safety_ratio: 0.8 # [-] jerk/acceleration ratio for safety + max_slow_down_jerk: -0.5 # [m/s^3] minimum jerk deceleration for safe brake. + max_slow_down_accel: -1.8 # [m/s^2] minimum accel deceleration for safe brake. + non_effective_jerk: -0.3 # [m/s^3] weak jerk for velocity planning. + non_effective_acceleration: -1.0 # [m/s^2] weak deceleration for velocity planning. + min_allowed_velocity: 1.0 # [m/s] minimum velocity allowed + safe_margin: 2.0 # [m] margin for detection failure(0.5m) + pedestrian radius(0.5m) + safe margin(1.0m) + detection_area: + min_occlusion_spot_size: 1.0 # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle. + slice_length: 10.0 # [m] size of slices in both length and distance relative to the ego path. + min_longitudinal_offset: 1.0 # [m] detection area safety buffer from front bumper. + max_lateral_distance: 6.0 # [m] buffer around the ego path used to build the detection area. + grid: + free_space_max: 40 # [-] maximum value of a free space cell in the occupancy grid + occupied_min: 60 # [-] minimum value of an occupied cell in the occupancy grid diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml new file mode 100644 index 0000000000000..a2b5ac5d5fcd1 --- /dev/null +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + stop_line: + stop_margin: 0.0 + stop_check_dist: 2.0 + stop_duration_sec: 1.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml new file mode 100644 index 0000000000000..f5db276c9ead8 --- /dev/null +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + traffic_light: + stop_margin: 0.0 + tl_state_timeout: 1.0 + external_tl_state_timeout: 1.0 + yellow_lamp_period: 2.75 + enable_pass_judge: true diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_traffic_light.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_traffic_light.param.yaml new file mode 100644 index 0000000000000..267dde50c7970 --- /dev/null +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_traffic_light.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + virtual_traffic_light: + max_delay_sec: 3.0 + near_line_distance: 1.0 + dead_line_margin: 1.0 + max_yaw_deviation_deg: 90.0 + check_timeout_after_stop_line: true 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 index f81d631db5040..7e5e575d6e001 100644 --- 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 @@ -138,6 +138,7 @@ def launch_setup(context, *args, **kwargs): ("~/input/vector_map", LaunchConfiguration("map_topic_name")), ("~/input/perception", "/perception/object_recognition/objects"), ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/scenario", "/planning/scenario_planning/scenario"), ( "~/input/external_approval", "/planning/scenario_planning/lane_driving/behavior_planning/" @@ -189,72 +190,108 @@ def launch_setup(context, *args, **kwargs): # behavior velocity planner blind_spot_param_path = os.path.join( - get_package_share_directory("behavior_velocity_planner"), + get_package_share_directory("tier4_planning_launch"), "config", + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_velocity_planner", "blind_spot.param.yaml", ) with open(blind_spot_param_path, "r") as f: blind_spot_param = yaml.safe_load(f)["/**"]["ros__parameters"] crosswalk_param_path = os.path.join( - get_package_share_directory("behavior_velocity_planner"), + get_package_share_directory("tier4_planning_launch"), "config", + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_velocity_planner", "crosswalk.param.yaml", ) with open(crosswalk_param_path, "r") as f: crosswalk_param = yaml.safe_load(f)["/**"]["ros__parameters"] detection_area_param_path = os.path.join( - get_package_share_directory("behavior_velocity_planner"), + get_package_share_directory("tier4_planning_launch"), "config", + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_velocity_planner", "detection_area.param.yaml", ) with open(detection_area_param_path, "r") as f: detection_area_param = yaml.safe_load(f)["/**"]["ros__parameters"] intersection_param_path = os.path.join( - get_package_share_directory("behavior_velocity_planner"), + get_package_share_directory("tier4_planning_launch"), "config", + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_velocity_planner", "intersection.param.yaml", ) with open(intersection_param_path, "r") as f: intersection_param = yaml.safe_load(f)["/**"]["ros__parameters"] stop_line_param_path = os.path.join( - get_package_share_directory("behavior_velocity_planner"), + get_package_share_directory("tier4_planning_launch"), "config", + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_velocity_planner", "stop_line.param.yaml", ) with open(stop_line_param_path, "r") as f: stop_line_param = yaml.safe_load(f)["/**"]["ros__parameters"] traffic_light_param_path = os.path.join( - get_package_share_directory("behavior_velocity_planner"), + get_package_share_directory("tier4_planning_launch"), "config", + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_velocity_planner", "traffic_light.param.yaml", ) with open(traffic_light_param_path, "r") as f: traffic_light_param = yaml.safe_load(f)["/**"]["ros__parameters"] virtual_traffic_light_param_path = os.path.join( - get_package_share_directory("behavior_velocity_planner"), + get_package_share_directory("tier4_planning_launch"), "config", + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_velocity_planner", "virtual_traffic_light.param.yaml", ) with open(virtual_traffic_light_param_path, "r") as f: virtual_traffic_light_param = yaml.safe_load(f)["/**"]["ros__parameters"] occlusion_spot_param_path = os.path.join( - get_package_share_directory("behavior_velocity_planner"), + get_package_share_directory("tier4_planning_launch"), "config", + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_velocity_planner", "occlusion_spot.param.yaml", ) with open(occlusion_spot_param_path, "r") as f: occlusion_spot_param = yaml.safe_load(f)["/**"]["ros__parameters"] no_stopping_area_param_path = os.path.join( - get_package_share_directory("behavior_velocity_planner"), + get_package_share_directory("tier4_planning_launch"), "config", + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_velocity_planner", "no_stopping_area.param.yaml", ) with open(no_stopping_area_param_path, "r") as f: 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 b7dc9ea3414f4..c6783d54e62bd 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -46,6 +46,10 @@ class GyroOdometer : public rclcpp::Node vehicle_twist_with_cov_sub_; rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Publisher::SharedPtr twist_raw_pub_; + rclcpp::Publisher::SharedPtr + twist_with_covariance_raw_pub_; + rclcpp::Publisher::SharedPtr twist_pub_; rclcpp::Publisher::SharedPtr twist_with_covariance_pub_; diff --git a/localization/gyro_odometer/launch/gyro_odometer.launch.xml b/localization/gyro_odometer/launch/gyro_odometer.launch.xml index afa27c0067998..f9de1ac2086f2 100644 --- a/localization/gyro_odometer/launch/gyro_odometer.launch.xml +++ b/localization/gyro_odometer/launch/gyro_odometer.launch.xml @@ -3,6 +3,9 @@ + + + @@ -14,6 +17,9 @@ + + + diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index 9e57abbf7cedb..2068e5a319177 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -34,6 +34,10 @@ GyroOdometer::GyroOdometer() imu_sub_ = create_subscription( "imu", rclcpp::QoS{100}, std::bind(&GyroOdometer::callbackImu, this, std::placeholders::_1)); + twist_raw_pub_ = create_publisher("twist_raw", rclcpp::QoS{10}); + twist_with_covariance_raw_pub_ = create_publisher( + "twist_with_covariance_raw", rclcpp::QoS{10}); + twist_pub_ = create_publisher("twist", rclcpp::QoS{10}); twist_with_covariance_pub_ = create_publisher( "twist_with_covariance", rclcpp::QoS{10}); @@ -93,15 +97,6 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m transformed_angular_velocity.header = tf_base2imu_ptr->header; tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_base2imu_ptr); - // clear imu yaw bias if vehicle is stopped - if ( - std::fabs(transformed_angular_velocity.vector.z) < 0.01 && - std::fabs(twist_with_cov_msg_ptr_->twist.twist.linear.x) < 0.01) { - transformed_angular_velocity.vector.x = 0.0; - transformed_angular_velocity.vector.y = 0.0; - transformed_angular_velocity.vector.z = 0.0; - } - // TODO(YamatoAndo) move code geometry_msgs::msg::TwistStamped twist; twist.header.stamp = imu_msg_ptr_->header.stamp; @@ -110,7 +105,7 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m twist.twist.angular.x = transformed_angular_velocity.vector.x; twist.twist.angular.y = transformed_angular_velocity.vector.y; twist.twist.angular.z = transformed_angular_velocity.vector.z; - twist_pub_->publish(twist); + twist_raw_pub_->publish(twist); geometry_msgs::msg::TwistWithCovarianceStamped twist_with_covariance; twist_with_covariance.header.stamp = imu_msg_ptr_->header.stamp; @@ -130,6 +125,21 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m twist_with_covariance.twist.covariance[28] = imu_msg_ptr_->angular_velocity_covariance[4]; twist_with_covariance.twist.covariance[35] = imu_msg_ptr_->angular_velocity_covariance[8]; + twist_with_covariance_raw_pub_->publish(twist_with_covariance); + + // clear imu yaw bias if vehicle is stopped + if ( + std::fabs(transformed_angular_velocity.vector.z) < 0.01 && + std::fabs(twist_with_cov_msg_ptr_->twist.twist.linear.x) < 0.01) { + twist.twist.angular.x = 0.0; + twist.twist.angular.y = 0.0; + twist.twist.angular.z = 0.0; + twist_with_covariance.twist.twist.angular.x = 0.0; + twist_with_covariance.twist.twist.angular.y = 0.0; + twist_with_covariance.twist.twist.angular.z = 0.0; + } + + twist_pub_->publish(twist); twist_with_covariance_pub_->publish(twist_with_covariance); } diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index 20deb7a785c0f..a85fb5d120b73 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -178,6 +178,10 @@ void ScanGroundFilterComponent::classifyPointCloud( } else { calculate_slope = true; } + if (is_point_close_to_prev) { + height_from_gnd = p->orig_point->z - ground_cluster.getAverageHeight(); + radius_distance_from_gnd = p->radius - ground_cluster.getAverageRadius(); + } if (calculate_slope) { // far from the previous point diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt new file mode 100644 index 0000000000000..0db1817dbe018 --- /dev/null +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -0,0 +1,54 @@ +cmake_minimum_required(VERSION 3.8) +project(image_projection_based_fusion) + +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_lint_auto REQUIRED) +find_package(ament_cmake_auto REQUIRED) +find_package(OpenCV REQUIRED) +find_package(Eigen3 REQUIRED) + +ament_auto_find_build_dependencies() + +include_directories( + include + ${OpenCV_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/fusion_node.cpp + src/debugger.cpp + src/utils/geometry.cpp + src/utils/utils.cpp + src/roi_cluster_fusion/node.cpp + src/roi_detected_object_fusion/node.cpp +) +target_link_libraries(${PROJECT_NAME} + ${OpenCV_LIBRARIES} + ${EIGEN3_LIBRARIES} +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "image_projection_based_fusion::RoiClusterFusionNode" + EXECUTABLE roi_cluster_fusion_node +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "image_projection_based_fusion::RoiDetectedObjectFusionNode" + EXECUTABLE roi_detected_object_fusion_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/perception/image_projection_based_fusion/README.md b/perception/image_projection_based_fusion/README.md new file mode 100644 index 0000000000000..9038c7a93a479 --- /dev/null +++ b/perception/image_projection_based_fusion/README.md @@ -0,0 +1,14 @@ +# image_projection_based_fusion + +## Purpose + +The `image_projection_based_fusion` is a package to fuse detected obstacles (bounding box or segmentation) from image and 3d pointcloud or obstacles (bounding box, cluster or segmentation). + +## Inner-workings / Algorithms + +Detail description of each fusion's algorithm is in the following links. + +| Fusion Name | Description | Detail | +| -------------------------- | ----------------------------------------------------------------------------------------------- | -------------------------------------------- | +| roi_cluster_fusion | Overwrite a classification label of clusters by that of ROIs from a 2D object detector. | [link](./docs/roi-cluster-fusion.md) | +| roi_detected_object_fusion | Overwrite a classification label of detected objects by that of ROIs from a 2D object detector. | [link](./docs/roi-detected-object-fusion.md) | diff --git a/perception/roi_cluster_fusion/images/roi_cluster_fusion.png b/perception/image_projection_based_fusion/docs/images/roi_cluster_fusion.png similarity index 100% rename from perception/roi_cluster_fusion/images/roi_cluster_fusion.png rename to perception/image_projection_based_fusion/docs/images/roi_cluster_fusion.png diff --git a/perception/roi_cluster_fusion/README.md b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md similarity index 81% rename from perception/roi_cluster_fusion/README.md rename to perception/image_projection_based_fusion/docs/roi-cluster-fusion.md index 58f7780d77967..2a4c881ca1539 100644 --- a/perception/roi_cluster_fusion/README.md +++ b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md @@ -2,7 +2,7 @@ ## Purpose -roi_cluster_fusion is a package for filtering clusters that are less likely to be objects and overwriting labels of clusters with that of Region Of Interests (ROIs) by a 2D object detector. +The `roi_cluster_fusion` is a package for filtering clusters that are less likely to be objects and overwriting labels of clusters with that of Region Of Interests (ROIs) by a 2D object detector. ## Inner-workings / Algorithms @@ -16,16 +16,17 @@ The clusters are projected onto image planes, and then if the ROIs of clusters a | Name | Type | Description | | --------------------- | -------------------------------------------------------- | ---------------------------------------------------------------------------------- | -| `clusters` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | clustered pointcloud | +| `input` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | clustered pointcloud | | `input/camera_infoID` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes, `ID` is between 0 and 7 | | `input/roisID` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | ROIs from each image, `ID` is between 0 and 7 | | `input/image_rawID` | `sensor_msgs::msg::Image` | images for visualization, `ID` is between 0 and 7 | ### Output -| Name | Type | Description | -| -------------------- | ------------------------- | ------------------------------------------------- | -| `output/image_rawID` | `sensor_msgs::msg::Image` | images for visualization, `ID` is between 0 and 7 | +| Name | Type | Description | +| -------------------- | -------------------------------------------------------- | ------------------------------------------------- | +| `output` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | labeled cluster pointcloud | +| `output/image_rawID` | `sensor_msgs::msg::Image` | images for visualization, `ID` is between 0 and 7 | ## Parameters @@ -39,6 +40,7 @@ The clusters are projected onto image planes, and then if the ROIs of clusters a | `use_cluster_semantic_type` | bool | if `false`, the labels of clusters are overwritten by `UNKNOWN` before fusion | | `iou_threshold` | float | the IoU threshold to overwrite a label of clusters with a label of roi | | `rois_number` | int | the number of input rois | +| `debug_mode` | bool | If `true`, subscribe and publish images for visualization. | ## Assumptions / Known limits @@ -62,6 +64,7 @@ Example: @@ -59,14 +59,14 @@ - + - - + + diff --git a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml new file mode 100644 index 0000000000000..f6466e6512a09 --- /dev/null +++ b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml @@ -0,0 +1,75 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/roi_cluster_fusion/package.xml b/perception/image_projection_based_fusion/package.xml similarity index 68% rename from perception/roi_cluster_fusion/package.xml rename to perception/image_projection_based_fusion/package.xml index 22f24bc6401f0..2fa9c7308d7aa 100644 --- a/perception/roi_cluster_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -1,13 +1,16 @@ - - roi_cluster_fusion + + + image_projection_based_fusion 0.1.0 - The roi_cluster_fusion package + The image_projection_based_fusion package Yukihiro Saito + Yusuke Muramatsu Apache License 2.0 ament_cmake_auto + autoware_auto_perception_msgs cv_bridge image_transport message_filters diff --git a/perception/image_projection_based_fusion/src/debugger.cpp b/perception/image_projection_based_fusion/src/debugger.cpp new file mode 100644 index 0000000000000..e1254fc136304 --- /dev/null +++ b/perception/image_projection_based_fusion/src/debugger.cpp @@ -0,0 +1,109 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "image_projection_based_fusion/debugger.hpp" + +#include + +namespace +{ + +void drawRoiOnImage( + const cv::Mat & image, const sensor_msgs::msg::RegionOfInterest & roi, const int width, + const int height, const cv::Scalar & color) +{ + const auto left = std::max(0, static_cast(roi.x_offset)); + const auto top = std::max(0, static_cast(roi.y_offset)); + const auto right = std::min(static_cast(roi.x_offset + roi.width), width); + const auto bottom = std::min(static_cast(roi.y_offset + roi.height), height); + cv::rectangle(image, cv::Point(left, top), cv::Point(right, bottom), color, 2); +} + +} // namespace + +namespace image_projection_based_fusion +{ +Debugger::Debugger(rclcpp::Node * node_ptr, const std::size_t image_num) : node_ptr_(node_ptr) +{ + image_buffers_.resize(image_num); + for (std::size_t img_i = 0; img_i < image_num; ++img_i) { + auto sub = image_transport::create_subscription( + node_ptr, "input/image_raw" + std::to_string(img_i), + std::bind(&Debugger::imageCallback, this, std::placeholders::_1, img_i), "raw", + rmw_qos_profile_sensor_data); + image_subs_.push_back(sub); + + std::vector node_params = {"format", "jpeg_quality", "png_level"}; + for (const auto & param : node_params) { + if (node_ptr->has_parameter(param)) { + node_ptr->undeclare_parameter(param); + } + } + + auto pub = + image_transport::create_publisher(node_ptr, "output/image_raw" + std::to_string(img_i)); + image_pubs_.push_back(pub); + image_buffers_.at(img_i).set_capacity(image_buffer_size); + } +} + +void Debugger::imageCallback( + const sensor_msgs::msg::Image::ConstSharedPtr input_image_msg, const std::size_t image_id) +{ + image_buffers_.at(image_id).push_front(input_image_msg); +} + +void Debugger::clear() +{ + image_rois_.clear(); + obstacle_rois_.clear(); + obstacle_points_.clear(); +} + +void Debugger::publishImage(const std::size_t image_id, const rclcpp::Time & stamp) +{ + const boost::circular_buffer & image_buffer = + image_buffers_.at(image_id); + const image_transport::Publisher & image_pub = image_pubs_.at(image_id); + + for (std::size_t i = 0; i < image_buffer.size(); ++i) { + if (image_buffer.at(i)->header.stamp != stamp) { + continue; + } + + auto cv_ptr = cv_bridge::toCvCopy(image_buffer.at(i), image_buffer.at(i)->encoding); + + for (const auto & point : obstacle_points_) { + cv::circle( + cv_ptr->image, cv::Point(static_cast(point.x()), static_cast(point.y())), 2, + cv::Scalar(255, 255, 255), 3, 4); + } + for (const auto & roi : obstacle_rois_) { + drawRoiOnImage( + cv_ptr->image, roi, image_buffer.at(i)->width, image_buffer.at(i)->height, + cv::Scalar(255, 0, 0)); + } + // TODO(yukke42): show iou_score on image + for (const auto & roi : image_rois_) { + drawRoiOnImage( + cv_ptr->image, roi, image_buffer.at(i)->width, image_buffer.at(i)->height, + cv::Scalar(0, 0, 255)); + } + + image_pub.publish(cv_ptr->toImageMsg()); + break; + } +} + +} // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp new file mode 100644 index 0000000000000..4e9aeddfa185e --- /dev/null +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -0,0 +1,224 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#define EIGEN_MPL2_ONLY + +#include "image_projection_based_fusion/fusion_node.hpp" + +#include +#include + +#include + +#include + +#include +#include + +namespace image_projection_based_fusion +{ + +template +FusionNode::FusionNode(const std::string & node_name, const rclcpp::NodeOptions & options) +: Node(node_name, options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) +{ + rois_number_ = static_cast(declare_parameter("rois_number", 1)); + if (rois_number_ < 1) { + RCLCPP_WARN( + this->get_logger(), "minimum rois_number is 1. current rois_number is %zu", rois_number_); + rois_number_ = 1; + } + if (rois_number_ > 8) { + RCLCPP_WARN( + this->get_logger(), "maximum rois_number is 8. current rois_number is %zu", rois_number_); + rois_number_ = 8; + } + + // subscribers + sub_.subscribe(this, "input", rclcpp::QoS(1).get_rmw_qos_profile()); + + camera_info_subs_.resize(rois_number_); + for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { + std::function fnc = + std::bind(&FusionNode::cameraInfoCallback, this, std::placeholders::_1, roi_i); + camera_info_subs_.at(roi_i) = this->create_subscription( + "input/camera_info" + std::to_string(roi_i), rclcpp::QoS{1}.best_effort(), fnc); + } + + rois_subs_.resize(rois_number_); + for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { + rois_subs_.at(roi_i) = + std::make_shared>( + this, "input/rois" + std::to_string(roi_i), rclcpp::QoS{1}.get_rmw_qos_profile()); + } + + // add dummy callback to enable passthrough filter + rois_subs_.at(0)->registerCallback( + std::bind(&FusionNode::dummyCallback, this, std::placeholders::_1)); + switch (rois_number_) { + case 1: + sync_ptr_ = std::make_shared( + SyncPolicy(10), sub_, *rois_subs_.at(0), passthrough_, passthrough_, passthrough_, + passthrough_, passthrough_, passthrough_, passthrough_); + break; + case 2: + sync_ptr_ = std::make_shared( + SyncPolicy(10), sub_, *rois_subs_.at(0), *rois_subs_.at(1), passthrough_, passthrough_, + passthrough_, passthrough_, passthrough_, passthrough_); + break; + case 3: + sync_ptr_ = std::make_shared( + SyncPolicy(10), sub_, *rois_subs_.at(0), *rois_subs_.at(1), *rois_subs_.at(2), passthrough_, + passthrough_, passthrough_, passthrough_, passthrough_); + break; + case 4: + sync_ptr_ = std::make_shared( + SyncPolicy(10), sub_, *rois_subs_.at(0), *rois_subs_.at(1), *rois_subs_.at(2), + *rois_subs_.at(3), passthrough_, passthrough_, passthrough_, passthrough_); + break; + case 5: + sync_ptr_ = std::make_shared( + SyncPolicy(10), sub_, *rois_subs_.at(0), *rois_subs_.at(1), *rois_subs_.at(2), + *rois_subs_.at(3), *rois_subs_.at(4), passthrough_, passthrough_, passthrough_); + break; + case 6: + sync_ptr_ = std::make_shared( + SyncPolicy(10), sub_, *rois_subs_.at(0), *rois_subs_.at(1), *rois_subs_.at(2), + *rois_subs_.at(3), *rois_subs_.at(4), *rois_subs_.at(5), passthrough_, passthrough_); + break; + case 7: + sync_ptr_ = std::make_shared( + SyncPolicy(10), sub_, *rois_subs_.at(0), *rois_subs_.at(1), *rois_subs_.at(2), + *rois_subs_.at(3), *rois_subs_.at(4), *rois_subs_.at(5), *rois_subs_.at(6), passthrough_); + break; + case 8: + sync_ptr_ = std::make_shared( + SyncPolicy(10), sub_, *rois_subs_.at(0), *rois_subs_.at(1), *rois_subs_.at(2), + *rois_subs_.at(3), *rois_subs_.at(4), *rois_subs_.at(5), *rois_subs_.at(6), + *rois_subs_.at(7)); + default: + return; + } + + sync_ptr_->registerCallback(std::bind( + &FusionNode::fusionCallback, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, + std::placeholders::_7, std::placeholders::_8, std::placeholders::_9)); + + // publisher + pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); + + // debugger + if (declare_parameter("debug_mode", false)) { + debugger_ = std::make_shared(this, rois_number_); + } +} + +template +void FusionNode::cameraInfoCallback( + const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, + const std::size_t camera_id) +{ + camera_info_map_[camera_id] = *input_camera_info_msg; +} + +template +void FusionNode::preprocess(Msg & ouput_msg __attribute__((unused))) +{ + // do nothing by default +} + +template +void FusionNode::fusionCallback( + typename Msg::ConstSharedPtr input_msg, DetectedObjectsWithFeature::ConstSharedPtr input_roi0_msg, + DetectedObjectsWithFeature::ConstSharedPtr input_roi1_msg, + DetectedObjectsWithFeature::ConstSharedPtr input_roi2_msg, + DetectedObjectsWithFeature::ConstSharedPtr input_roi3_msg, + DetectedObjectsWithFeature::ConstSharedPtr input_roi4_msg, + DetectedObjectsWithFeature::ConstSharedPtr input_roi5_msg, + DetectedObjectsWithFeature::ConstSharedPtr input_roi6_msg, + DetectedObjectsWithFeature::ConstSharedPtr input_roi7_msg) +{ + if (pub_ptr_->get_subscription_count() < 1) { + return; + } + + Msg output_msg = *input_msg; + + preprocess(output_msg); + + for (std::size_t image_id = 0; image_id < rois_subs_.size(); ++image_id) { + DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg; + switch (image_id) { + case 0: + input_roi_msg = input_roi0_msg; + break; + case 1: + input_roi_msg = input_roi1_msg; + break; + case 2: + input_roi_msg = input_roi2_msg; + break; + case 3: + input_roi_msg = input_roi3_msg; + break; + case 4: + input_roi_msg = input_roi4_msg; + break; + case 5: + input_roi_msg = input_roi5_msg; + break; + case 6: + input_roi_msg = input_roi6_msg; + break; + case 7: + input_roi_msg = input_roi7_msg; + break; + default: + RCLCPP_ERROR(this->get_logger(), "invalid image id. id is %zu", image_id); + return; + } + + if (camera_info_map_.find(image_id) == camera_info_map_.end()) { + RCLCPP_WARN(this->get_logger(), "no camera info. id is %zu", image_id); + continue; + } + if (debugger_) { + debugger_->clear(); + } + + fuseOnSingleImage( + *input_msg, image_id, *input_roi_msg, camera_info_map_.at(image_id), output_msg); + } + + postprocess(); + + publish(output_msg); +} + +template +void FusionNode::postprocess() +{ + // do nothing by default +} + +template +void FusionNode::publish(const Msg & output_msg) +{ + pub_ptr_->publish(output_msg); +} + +template class FusionNode; +template class FusionNode; +} // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp new file mode 100644 index 0000000000000..f219670a9e93a --- /dev/null +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -0,0 +1,173 @@ +// 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. + +#include "image_projection_based_fusion/roi_cluster_fusion/node.hpp" + +#include +#include + +#include +#include + +#include +#include + +namespace image_projection_based_fusion +{ + +RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) +: FusionNode("roi_cluster_fusion", options) +{ + use_iou_x_ = declare_parameter("use_iou_x", true); + use_iou_y_ = declare_parameter("use_iou_y", false); + use_iou_ = declare_parameter("use_iou", false); + use_cluster_semantic_type_ = declare_parameter("use_cluster_semantic_type", false); + iou_threshold_ = declare_parameter("iou_threshold", 0.1); +} + +void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) +{ + // reset cluster semantic type + if (!use_cluster_semantic_type_) { + for (auto & feature_object : output_cluster_msg.feature_objects) { + feature_object.object.classification.front().label = + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + feature_object.object.existence_probability = 0.0; + } + } +} + +void RoiClusterFusionNode::fuseOnSingleImage( + const DetectedObjectsWithFeature & input_cluster_msg, const std::size_t image_id, + const DetectedObjectsWithFeature & input_roi_msg, + const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg) +{ + std::vector debug_image_rois; + std::vector debug_pointcloud_rois; + std::vector debug_image_points; + + Eigen::Matrix4d projection; + projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3), + camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7), + camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11); + + // get transform from cluster frame id to camera optical frame id + geometry_msgs::msg::TransformStamped transform_stamped; + { + const auto transform_stamped_optional = getTransformStamped( + tf_buffer_, /*target*/ camera_info.header.frame_id, + /*source*/ input_cluster_msg.header.frame_id, camera_info.header.stamp); + if (!transform_stamped_optional) { + return; + } + transform_stamped = transform_stamped_optional.value(); + } + + std::map m_cluster_roi; + for (std::size_t i = 0; i < input_cluster_msg.feature_objects.size(); ++i) { + if (input_cluster_msg.feature_objects.at(i).feature.cluster.data.empty()) { + continue; + } + + sensor_msgs::msg::PointCloud2 transformed_cluster; + tf2::doTransform( + input_cluster_msg.feature_objects.at(i).feature.cluster, transformed_cluster, + transform_stamped); + + int min_x(camera_info.width), min_y(camera_info.height), max_x(0), max_y(0); + std::vector projected_points; + projected_points.reserve(transformed_cluster.data.size()); + for (sensor_msgs::PointCloud2ConstIterator iter_x(transformed_cluster, "x"), + iter_y(transformed_cluster, "y"), iter_z(transformed_cluster, "z"); + iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { + if (*iter_z <= 0.0) { + continue; + } + + Eigen::Vector4d projected_point = + projection * Eigen::Vector4d(*iter_x, *iter_y, *iter_z, 1.0); + Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( + projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); + if ( + 0 <= static_cast(normalized_projected_point.x()) && + static_cast(normalized_projected_point.x()) <= + static_cast(camera_info.width) - 1 && + 0 <= static_cast(normalized_projected_point.y()) && + static_cast(normalized_projected_point.y()) <= + static_cast(camera_info.height) - 1) { + min_x = std::min(static_cast(normalized_projected_point.x()), min_x); + min_y = std::min(static_cast(normalized_projected_point.y()), min_y); + max_x = std::max(static_cast(normalized_projected_point.x()), max_x); + max_y = std::max(static_cast(normalized_projected_point.y()), max_y); + projected_points.push_back(normalized_projected_point); + debug_image_points.push_back(normalized_projected_point); + } + } + if (projected_points.empty()) { + continue; + } + + sensor_msgs::msg::RegionOfInterest roi; + // roi.do_rectify = m_camera_info_.at(id).do_rectify; + roi.x_offset = min_x; + roi.y_offset = min_y; + roi.width = max_x - min_x; + roi.height = max_y - min_y; + m_cluster_roi.insert(std::make_pair(i, roi)); + debug_pointcloud_rois.push_back(roi); + } + + for (const auto & feature_obj : input_roi_msg.feature_objects) { + int index = 0; + double max_iou = 0.0; + for (const auto & cluster_map : m_cluster_roi) { + double iou(0.0), iou_x(0.0), iou_y(0.0); + if (use_iou_) { + iou = calcIoU(cluster_map.second, feature_obj.feature.roi); + } + if (use_iou_x_) { + iou_x = calcIoUX(cluster_map.second, feature_obj.feature.roi); + } + if (use_iou_y_) { + iou_y = calcIoUY(cluster_map.second, feature_obj.feature.roi); + } + if (max_iou < iou + iou_x + iou_y) { + index = cluster_map.first; + max_iou = iou + iou_x + iou_y; + } + } + if ( + iou_threshold_ < max_iou && + output_cluster_msg.feature_objects.at(index).object.existence_probability <= + feature_obj.object.existence_probability && + feature_obj.object.classification.front().label != + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) { + output_cluster_msg.feature_objects.at(index).object.classification = + feature_obj.object.classification; + } + debug_image_rois.push_back(feature_obj.feature.roi); + } + + if (debugger_) { + debugger_->image_rois_ = debug_image_rois; + debugger_->obstacle_rois_ = debug_pointcloud_rois; + debugger_->obstacle_points_ = debug_image_points; + debugger_->publishImage(image_id, input_roi_msg.header.stamp); + } +} + +} // namespace image_projection_based_fusion + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(image_projection_based_fusion::RoiClusterFusionNode) diff --git a/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp new file mode 100644 index 0000000000000..ec9183f237978 --- /dev/null +++ b/perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -0,0 +1,177 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "image_projection_based_fusion/roi_detected_object_fusion/node.hpp" + +#include +#include + +namespace image_projection_based_fusion +{ + +RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) +: FusionNode("roi_detected_object_fusion", options) +{ + use_iou_x_ = declare_parameter("use_iou_x", false); + use_iou_y_ = declare_parameter("use_iou_y", false); + use_iou_ = declare_parameter("use_iou", false); + iou_threshold_ = declare_parameter("iou_threshold", 0.1); +} + +void RoiDetectedObjectFusionNode::fuseOnSingleImage( + const DetectedObjects & input_object_msg, const std::size_t image_id, + const DetectedObjectsWithFeature & input_roi_msg, + const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjects & output_object_msg) +{ + Eigen::Affine3d object2camera_affine; + { + const auto transform_stamped_optional = getTransformStamped( + tf_buffer_, /*target*/ camera_info.header.frame_id, + /*source*/ input_object_msg.header.frame_id, camera_info.header.stamp); + if (!transform_stamped_optional) { + return; + } + object2camera_affine = transformToEigen(transform_stamped_optional.value().transform); + } + + Eigen::Matrix4d camera_projection; + camera_projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), + camera_info.p.at(3), camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), + camera_info.p.at(7), camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), + camera_info.p.at(11); + + std::map object_roi_map; + generateDetectedObjectRois( + input_object_msg.objects, static_cast(camera_info.width), + static_cast(camera_info.height), object2camera_affine, camera_projection, + object_roi_map); + updateDetectedObjectClassification( + input_roi_msg.feature_objects, object_roi_map, output_object_msg.objects); + + if (debugger_) { + debugger_->image_rois_.reserve(input_roi_msg.feature_objects.size()); + for (std::size_t roi_i = 0; roi_i < input_roi_msg.feature_objects.size(); ++roi_i) { + debugger_->image_rois_.push_back(input_roi_msg.feature_objects.at(roi_i).feature.roi); + } + debugger_->publishImage(image_id, input_roi_msg.header.stamp); + } +} + +void RoiDetectedObjectFusionNode::generateDetectedObjectRois( + const std::vector & input_objects, const double image_width, + const double image_height, const Eigen::Affine3d & object2camera_affine, + const Eigen::Matrix4d & camera_projection, + std::map & object_roi_map) +{ + for (std::size_t obj_i = 0; obj_i < input_objects.size(); ++obj_i) { + std::vector vertices_camera_coord; + { + const auto & object = input_objects.at(obj_i); + std::vector vertices; + objectToVertices(object.kinematics.pose_with_covariance.pose, object.shape, vertices); + transformPoints(vertices, object2camera_affine, vertices_camera_coord); + } + + double min_x(std::numeric_limits::max()), min_y(std::numeric_limits::max()), + max_x(std::numeric_limits::min()), max_y(std::numeric_limits::min()); + std::size_t point_on_image_cnt = 0; + for (const auto & point : vertices_camera_coord) { + if (point.z() <= 0.0) { + continue; + } + + Eigen::Vector2d proj_point; + { + Eigen::Vector4d proj_point_hom = + camera_projection * Eigen::Vector4d(point.x(), point.y(), point.z(), 1.0); + proj_point = Eigen::Vector2d( + proj_point_hom.x() / (proj_point_hom.z()), proj_point_hom.y() / (proj_point_hom.z())); + } + + min_x = std::min(proj_point.x(), min_x); + min_y = std::min(proj_point.y(), min_y); + max_x = std::max(proj_point.x(), max_x); + max_y = std::max(proj_point.y(), max_y); + + if ( + proj_point.x() >= 0 && proj_point.x() <= image_width - 1 && proj_point.y() >= 0 && + proj_point.y() <= image_height - 1) { + point_on_image_cnt++; + + if (debugger_) { + debugger_->obstacle_points_.push_back(proj_point); + } + } + } + if (point_on_image_cnt == 0) { + continue; + } + + min_x = std::max(min_x, 0.0); + min_y = std::max(min_y, 0.0); + max_x = std::min(max_x, image_width - 1); + max_y = std::min(max_y, image_height - 1); + + // build roi + sensor_msgs::msg::RegionOfInterest roi; + roi.x_offset = static_cast(min_x); + roi.y_offset = static_cast(min_y); + roi.width = static_cast(max_x) - static_cast(min_x); + roi.height = static_cast(max_y) - static_cast(min_y); + object_roi_map.insert(std::make_pair(obj_i, roi)); + + if (debugger_) { + debugger_->obstacle_rois_.push_back(roi); + } + } +} + +void RoiDetectedObjectFusionNode::updateDetectedObjectClassification( + const std::vector & image_rois, + const std::map & object_roi_map, + std::vector & output_objects) +{ + for (const auto & image_roi : image_rois) { + std::size_t object_i = 0; + double max_iou = 0.0; + for (const auto & object_map : object_roi_map) { + double iou(0.0), iou_x(0.0), iou_y(0.0); + if (use_iou_) { + iou = calcIoU(object_map.second, image_roi.feature.roi); + } + if (use_iou_x_) { + iou_x = calcIoUX(object_map.second, image_roi.feature.roi); + } + if (use_iou_y_) { + iou_y = calcIoUY(object_map.second, image_roi.feature.roi); + } + + if (iou + iou_x + iou_y > max_iou) { + object_i = object_map.first; + max_iou = iou + iou_x + iou_y; + } + } + + if ( + max_iou > iou_threshold_ && + output_objects.at(object_i).existence_probability <= image_roi.object.existence_probability) { + output_objects.at(object_i).classification = image_roi.object.classification; + } + } +} + +} // namespace image_projection_based_fusion + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(image_projection_based_fusion::RoiDetectedObjectFusionNode) diff --git a/perception/image_projection_based_fusion/src/utils/geometry.cpp b/perception/image_projection_based_fusion/src/utils/geometry.cpp new file mode 100644 index 0000000000000..a365f6b0decd9 --- /dev/null +++ b/perception/image_projection_based_fusion/src/utils/geometry.cpp @@ -0,0 +1,165 @@ +// 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. + +#include "image_projection_based_fusion/utils/geometry.hpp" + +#include + +namespace image_projection_based_fusion +{ + +double calcIoU( + const sensor_msgs::msg::RegionOfInterest & roi_1, + const sensor_msgs::msg::RegionOfInterest & roi_2) +{ + double s_1, s_2; + s_1 = static_cast(roi_1.width * static_cast(roi_1.height)); + s_2 = static_cast(roi_2.width * static_cast(roi_2.height)); + + double overlap_s; + double overlap_max_x, overlap_max_y, overlap_min_x, overlap_min_y; + overlap_min_x = roi_1.x_offset < roi_2.x_offset ? roi_2.x_offset : roi_1.x_offset; + overlap_min_y = roi_1.y_offset < roi_2.y_offset ? roi_2.y_offset : roi_1.y_offset; + overlap_max_x = roi_1.x_offset + roi_1.width < roi_2.x_offset + roi_2.width + ? roi_1.x_offset + roi_1.width + : roi_2.x_offset + roi_2.width; + overlap_max_y = roi_1.y_offset + roi_1.height < roi_2.y_offset + roi_2.height + ? roi_1.y_offset + roi_1.height + : roi_2.y_offset + roi_2.height; + overlap_s = (overlap_max_x - overlap_min_x) * (overlap_max_y - overlap_min_y); + if (overlap_max_x < overlap_min_x || overlap_max_y < overlap_min_y) { + return 0.0; + } + return overlap_s / (s_1 + s_2 - overlap_s); +} +double calcIoUX( + const sensor_msgs::msg::RegionOfInterest & roi_1, + const sensor_msgs::msg::RegionOfInterest & roi_2) +{ + double s_1, s_2; + s_1 = static_cast(roi_1.width); + s_2 = static_cast(roi_2.width); + double overlap_s; + double overlap_max_x, overlap_max_y, overlap_min_x, overlap_min_y; + overlap_min_x = roi_1.x_offset < roi_2.x_offset ? roi_2.x_offset : roi_1.x_offset; + overlap_min_y = roi_1.y_offset < roi_2.y_offset ? roi_2.y_offset : roi_1.y_offset; + overlap_max_x = roi_1.x_offset + roi_1.width < roi_2.x_offset + roi_2.width + ? roi_1.x_offset + roi_1.width + : roi_2.x_offset + roi_2.width; + overlap_max_y = roi_1.y_offset + roi_1.height < roi_2.y_offset + roi_2.height + ? roi_1.y_offset + roi_1.height + : roi_2.y_offset + roi_2.height; + overlap_s = (overlap_max_x - overlap_min_x); + if (overlap_max_x < overlap_min_x || overlap_max_y < overlap_min_y) { + return 0.0; + } + return overlap_s / (s_1 + s_2 - overlap_s); +} +double calcIoUY( + const sensor_msgs::msg::RegionOfInterest & roi_1, + const sensor_msgs::msg::RegionOfInterest & roi_2) +{ + double s_1, s_2; + s_1 = static_cast(roi_1.height); + s_2 = static_cast(roi_2.height); + double overlap_s; + double overlap_max_x, overlap_max_y, overlap_min_x, overlap_min_y; + overlap_min_x = roi_1.x_offset < roi_2.x_offset ? roi_2.x_offset : roi_1.x_offset; + overlap_min_y = roi_1.y_offset < roi_2.y_offset ? roi_2.y_offset : roi_1.y_offset; + overlap_max_x = roi_1.x_offset + roi_1.width < roi_2.x_offset + roi_2.width + ? roi_1.x_offset + roi_1.width + : roi_2.x_offset + roi_2.width; + overlap_max_y = roi_1.y_offset + roi_1.height < roi_2.y_offset + roi_2.height + ? roi_1.y_offset + roi_1.height + : roi_2.y_offset + roi_2.height; + overlap_s = (overlap_max_y - overlap_min_y); + if (overlap_max_x < overlap_min_x || overlap_max_y < overlap_min_y) { + return 0.0; + } + return overlap_s / (s_1 + s_2 - overlap_s); +} + +void objectToVertices( + const Pose & pose, const Shape & shape, std::vector & vertices) +{ + if (shape.type == Shape::BOUNDING_BOX) { + boundingBoxToVertices(pose, shape, vertices); + } else if (shape.type == Shape::CYLINDER) { + cylinderToVertices(pose, shape, vertices); + } else if (shape.type == Shape::POLYGON) { + // polygonToVertices(pose, shape, vertices); + RCLCPP_WARN_STREAM( + rclcpp::get_logger("image_projection_based_fusion"), "POLYGON is not supported"); + } +} + +void boundingBoxToVertices( + const Pose & pose, const Shape & shape, std::vector & vertices) +{ + const std::vector> corners_template = { + // down surface + {1, 1, -1}, + {1, -1, -1}, + {-1, -1, -1}, + {-1, 1, -1}, + // up surface + {1, 1, 1}, + {1, -1, 1}, + {-1, -1, 1}, + {-1, 1, 1}, + }; + + const auto position = Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z); + const auto orientation = Eigen::Quaterniond( + pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); + + for (const auto & corner : corners_template) { + Eigen::Vector3d corner_point( + shape.dimensions.x * corner.at(0) / 2.0, shape.dimensions.y * corner.at(1) / 2.0, + shape.dimensions.z * corner.at(2) / 2.0); + vertices.push_back(orientation * corner_point + position); + } +} + +void cylinderToVertices( + const Pose & pose, const Shape & shape, std::vector & vertices) +{ + const auto & center = pose.position; + const auto & radius = shape.dimensions.x * 0.5; + constexpr std::size_t n = 6; + vertices.reserve(n * 2); + for (std::size_t i = 0; i < n; ++i) { + Eigen::Vector3d vertex; + const double theta = (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n); + vertex.x() = std::cos(theta) * radius + center.x; + vertex.y() = std::sin(theta) * radius + center.y; + vertex.z() = shape.dimensions.z * 0.5 + center.z; + vertices.push_back(vertex); + vertex.z() = -shape.dimensions.z * 0.5 + center.z; + vertices.push_back(vertex); + } +} + +void transformPoints( + const std::vector & input_points, const Eigen::Affine3d & affine_transform, + std::vector & output_points) +{ + output_points.reserve(input_points.size()); + for (const auto & point : input_points) { + output_points.push_back(affine_transform * point); + } +} + +} // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/utils/utils.cpp b/perception/image_projection_based_fusion/src/utils/utils.cpp new file mode 100644 index 0000000000000..1eaf04562b2b8 --- /dev/null +++ b/perception/image_projection_based_fusion/src/utils/utils.cpp @@ -0,0 +1,42 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "image_projection_based_fusion/utils/utils.hpp" + +namespace image_projection_based_fusion +{ + +std::optional getTransformStamped( + const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, + const std::string & source_frame_id, const rclcpp::Time & time) +{ + try { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped = tf_buffer.lookupTransform( + target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); + return transform_stamped; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("image_projection_based_fusion"), ex.what()); + return std::nullopt; + } +} + +Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t) +{ + Eigen::Affine3d a; + a.matrix() = tf2::transformToEigen(t).matrix(); + return a; +} + +} // namespace image_projection_based_fusion diff --git a/perception/map_based_prediction/CMakeLists.txt b/perception/map_based_prediction/CMakeLists.txt index 1e290e43a6330..044c451160cc4 100644 --- a/perception/map_based_prediction/CMakeLists.txt +++ b/perception/map_based_prediction/CMakeLists.txt @@ -34,5 +34,6 @@ endif() ament_auto_package( INSTALL_TO_SHARE + config launch ) diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index c944af87e4bc5..046e68dec0c0e 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -38,6 +38,7 @@ set(MULTI_OBJECT_TRACKER_SRC src/tracker/model/pedestrian_tracker.cpp src/tracker/model/pedestrian_and_bicycle_tracker.cpp src/tracker/model/unknown_tracker.cpp + src/tracker/model/pass_through_tracker.cpp src/data_association/data_association.cpp ) diff --git a/perception/multi_object_tracker/config/default_tracker.param.yaml b/perception/multi_object_tracker/config/default_tracker.param.yaml new file mode 100644 index 0000000000000..495df3a30d96e --- /dev/null +++ b/perception/multi_object_tracker/config/default_tracker.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + car_tracker: "multi_vehicle_tracker" + truck_tracker: "multi_vehicle_tracker" + bus_tracker: "multi_vehicle_tracker" + pedestrian_tracker: "pedestrian_and_bicycle_tracker" + bicycle_tracker: "pedestrian_and_bicycle_tracker" + motorcycle_tracker: "pedestrian_and_bicycle_tracker" diff --git a/perception/multi_object_tracker/config/simulation_tracker.param.yaml b/perception/multi_object_tracker/config/simulation_tracker.param.yaml new file mode 100644 index 0000000000000..325cfacc9a3a3 --- /dev/null +++ b/perception/multi_object_tracker/config/simulation_tracker.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + car_tracker: "pass_through_tracker" + truck_tracker: "pass_through_tracker" + bus_tracker: "pass_through_tracker" + pedestrian_tracker: "pass_through_tracker" + bicycle_tracker: "pass_through_tracker" + motorcycle_tracker: "pass_through_tracker" diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index d79180e4b2dd5..48e6bd2b0ec23 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -55,6 +56,8 @@ class MultiObjectTracker : public rclcpp::Node tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; + std::map tracker_map_; + void onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); void onTimer(); diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 991d9940a0f1e..6232c9f09bea7 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -54,6 +54,7 @@ class BigVehicleTracker : public Tracker float r_cov_x; float r_cov_y; float r_cov_yaw; + float r_cov_vx; float p0_cov_x; float p0_cov_y; float p0_cov_yaw; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index bdaef7309c916..5d6cac7430671 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -54,6 +54,7 @@ class NormalVehicleTracker : public Tracker float r_cov_x; float r_cov_y; float r_cov_yaw; + float r_cov_vx; float p0_cov_x; float p0_cov_y; float p0_cov_yaw; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp new file mode 100644 index 0000000000000..8c5033e2c3900 --- /dev/null +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -0,0 +1,47 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Yutaka Shimizu +// + +#ifndef MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ +#define MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ + +#include "tracker_base.hpp" + +#include + +class PassThroughTracker : public Tracker +{ +private: + autoware_auto_perception_msgs::msg::DetectedObject object_; + autoware_auto_perception_msgs::msg::DetectedObject prev_observed_object_; + rclcpp::Logger logger_; + rclcpp::Time last_update_time_; + +public: + PassThroughTracker( + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object); + bool predict(const rclcpp::Time & time) override; + bool measure( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const rclcpp::Time & time) override; + bool getTrackedObject( + const rclcpp::Time & time, + autoware_auto_perception_msgs::msg::TrackedObject & object) const override; + virtual ~PassThroughTracker() {} +}; + +#endif // MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/tracker.hpp index 75560750f07b6..a8bc58e254fc2 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/tracker.hpp @@ -23,6 +23,7 @@ #include "model/big_vehicle_tracker.hpp" #include "model/multiple_vehicle_tracker.hpp" #include "model/normal_vehicle_tracker.hpp" +#include "model/pass_through_tracker.hpp" #include "model/pedestrian_and_bicycle_tracker.hpp" #include "model/pedestrian_tracker.hpp" #include "model/tracker_base.hpp" diff --git a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml b/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml index 7b1a2cb31538a..db0e18e408a8b 100644 --- a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml +++ b/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml @@ -4,6 +4,7 @@ + @@ -11,6 +12,7 @@ + diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index d5c2946843365..c8b89ec3c518d 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -196,6 +196,20 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) const auto max_rad_matrix = this->declare_parameter>("max_rad_matrix"); const auto min_iou_matrix = this->declare_parameter>("min_iou_matrix"); + // tracker map + tracker_map_.insert( + std::make_pair(Label::CAR, this->declare_parameter("car_tracker"))); + tracker_map_.insert( + std::make_pair(Label::TRUCK, this->declare_parameter("truck_tracker"))); + tracker_map_.insert( + std::make_pair(Label::BUS, this->declare_parameter("bus_tracker"))); + tracker_map_.insert( + std::make_pair(Label::PEDESTRIAN, this->declare_parameter("pedestrian_tracker"))); + tracker_map_.insert( + std::make_pair(Label::BICYCLE, this->declare_parameter("bicycle_tracker"))); + tracker_map_.insert( + std::make_pair(Label::MOTORCYCLE, this->declare_parameter("motorcycle_tracker"))); + data_association_ = std::make_unique( can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix, min_iou_matrix); @@ -265,12 +279,22 @@ std::shared_ptr MultiObjectTracker::createNewTracker( const rclcpp::Time & time) const { const std::uint8_t label = utils::getHighestProbLabel(object.classification); - if (label == Label::CAR || label == Label::TRUCK || label == Label::BUS) { + const auto tracker = tracker_map_.at(label); + + if (tracker == "bicycle_tracker") { + return std::make_shared(time, object); + } else if (tracker == "big_vehicle_tracker") { + return std::make_shared(time, object); + } else if (tracker == "multi_vehicle_tracker") { return std::make_shared(time, object); - } else if (label == Label::PEDESTRIAN) { - return std::make_shared(time, object); - } else if (label == Label::BICYCLE || label == Label::MOTORCYCLE) { + } else if (tracker == "normal_vehicle_tracker") { + return std::make_shared(time, object); + } else if (tracker == "pass_through_tracker") { + return std::make_shared(time, object); + } else if (tracker == "pedestrian_and_bicycle_tracker") { return std::make_shared(time, object); + } else if (tracker == "pedestrian_tracker") { + return std::make_shared(time, object); } else { return std::make_shared(time, object); } diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 0c58c83ae722d..50a64fa7d5974 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -49,6 +49,7 @@ BigVehicleTracker::BigVehicleTracker( float r_stddev_x = 1.5; // [m] float r_stddev_y = 0.5; // [m] float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] + float r_stddev_vx = 1.0; // [m/s] float p0_stddev_x = 1.5; // [m] float p0_stddev_y = 0.5; // [m] float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] @@ -62,6 +63,7 @@ BigVehicleTracker::BigVehicleTracker( 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); @@ -230,7 +232,8 @@ bool BigVehicleTracker::measureWithPose( r_cov_y = ekf_params_.r_cov_y; } - constexpr int dim_y = 3; // pos x, pos y, yaw, depending on Pose output + const int dim_y = + object.kinematics.has_twist ? 4 : 3; // pos x, pos y, yaw, vx depending on Pose output double measurement_yaw = tier4_autoware_utils::normalizeRadian( tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); { @@ -247,17 +250,17 @@ bool BigVehicleTracker::measureWithPose( /* Set measurement matrix */ Eigen::MatrixXd Y(dim_y, 1); - Y << object.kinematics.pose_with_covariance.pose.position.x, - object.kinematics.pose_with_covariance.pose.position.y, measurement_yaw; - - /* Set measurement matrix */ Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); + + Y(IDX::X, 0) = object.kinematics.pose_with_covariance.pose.position.x; + Y(IDX::Y, 0) = object.kinematics.pose_with_covariance.pose.position.y; + Y(IDX::YAW, 0) = measurement_yaw; C(0, IDX::X) = 1.0; // for pos x C(1, IDX::Y) = 1.0; // for pos y C(2, IDX::YAW) = 1.0; // for yaw /* Set measurement noise covariance */ - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); if ( !ekf_params_.use_measurement_covariance || object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0 || @@ -282,6 +285,20 @@ bool BigVehicleTracker::measureWithPose( R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; } + + if (object.kinematics.has_twist) { + Y(IDX::VX, 0) = object.kinematics.twist_with_covariance.twist.linear.x; + C(3, IDX::VX) = 1.0; // for vx + + if ( + !ekf_params_.use_measurement_covariance || + object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0) { + R(3, 3) = ekf_params_.r_cov_vx; // vx -vx + } else { + R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + } + } + if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 818912aa7b6b8..3dddd2d8cc54d 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -49,6 +49,7 @@ NormalVehicleTracker::NormalVehicleTracker( float r_stddev_x = 1.0; // object coordinate [m] float r_stddev_y = 0.3; // object coordinate [m] float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] + float r_stddev_vx = 1.0; // object coordinate [m/s] float p0_stddev_x = 1.0; // object coordinate [m/s] float p0_stddev_y = 0.3; // object coordinate [m/s] float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] @@ -62,6 +63,7 @@ NormalVehicleTracker::NormalVehicleTracker( 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); @@ -230,7 +232,8 @@ bool NormalVehicleTracker::measureWithPose( r_cov_y = ekf_params_.r_cov_y; } - constexpr int dim_y = 3; // pos x, pos y, yaw, depending on Pose output + const int dim_y = + object.kinematics.has_twist ? 4 : 3; // pos x, pos y, yaw, vx depending on Pose output double measurement_yaw = tier4_autoware_utils::normalizeRadian( tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); { @@ -245,19 +248,19 @@ bool NormalVehicleTracker::measureWithPose( } } - /* Set measurement matrix */ + /* Set measurement matrix and noise covariance*/ Eigen::MatrixXd Y(dim_y, 1); - Y << object.kinematics.pose_with_covariance.pose.position.x, - object.kinematics.pose_with_covariance.pose.position.y, measurement_yaw; - - /* Set measurement matrix */ Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); + + Y(IDX::X, 0) = object.kinematics.pose_with_covariance.pose.position.x; + Y(IDX::Y, 0) = object.kinematics.pose_with_covariance.pose.position.y; + Y(IDX::YAW, 0) = measurement_yaw; C(0, IDX::X) = 1.0; // for pos x C(1, IDX::Y) = 1.0; // for pos y C(2, IDX::YAW) = 1.0; // for yaw /* Set measurement noise covariance */ - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); if ( !ekf_params_.use_measurement_covariance || object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0 || @@ -282,6 +285,21 @@ bool NormalVehicleTracker::measureWithPose( R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; } + + if (object.kinematics.has_twist) { + Y(IDX::VX, 0) = object.kinematics.twist_with_covariance.twist.linear.x; + C(3, IDX::VX) = 1.0; // for vx + + if ( + !ekf_params_.use_measurement_covariance || + object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X] == 0.0) { + R(3, 3) = ekf_params_.r_cov_vx; // vx -vx + } else { + R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + } + } + + // update if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } diff --git a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp new file mode 100644 index 0000000000000..8838e98ef373f --- /dev/null +++ b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp @@ -0,0 +1,105 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Yutaka Shimizu +// + +#include +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include "multi_object_tracker/tracker/model/pass_through_tracker.hpp" +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include + +PassThroughTracker::PassThroughTracker( + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object) +: Tracker(time, object.classification), + logger_(rclcpp::get_logger("PassThroughTracker")), + last_update_time_(time) +{ + object_ = object; + prev_observed_object_ = object; +} + +bool PassThroughTracker::predict(const rclcpp::Time & time) +{ + if (0.5 /*500msec*/ < 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()); + } + + return true; +} + +bool PassThroughTracker::measure( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time) +{ + prev_observed_object_ = object_; + object_ = object; + + // Update Velocity if the observed object does not have twist information + const double dt = (time - last_update_time_).seconds(); + if (!object_.kinematics.has_twist && dt > 1e-6) { + const double dx = object_.kinematics.pose_with_covariance.pose.position.x - + prev_observed_object_.kinematics.pose_with_covariance.pose.position.x; + const double dy = object_.kinematics.pose_with_covariance.pose.position.y - + prev_observed_object_.kinematics.pose_with_covariance.pose.position.y; + object_.kinematics.twist_with_covariance.twist.linear.x = std::hypot(dx, dy) / dt; + } + last_update_time_ = time; + + return true; +} + +bool PassThroughTracker::getTrackedObject( + const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const +{ + object = utils::toTrackedObject(object_); + object.object_id = getUUID(); + object.classification = getClassification(); + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X] = 0.0; + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y] = 0.0; + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X] = 0.0; + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y] = 0.0; + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Z_Z] = 0.0; + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = 0.0; + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = 0.0; + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW] = 0.0; + + // twist covariance + 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] = 0.0; + object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Z_Z] = 0.0; + object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = 0.0; + object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = 0.0; + object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW] = 0.0; + + const double dt = (time - last_update_time_).seconds(); + if (0.5 /*500msec*/ < dt) { + RCLCPP_WARN( + logger_, "There is a large gap between last updated time and current time. (%f)", + (time - last_update_time_).seconds()); + } + + return true; +} diff --git a/perception/roi_cluster_fusion/CMakeLists.txt b/perception/roi_cluster_fusion/CMakeLists.txt deleted file mode 100644 index 122b7ee4208a0..0000000000000 --- a/perception/roi_cluster_fusion/CMakeLists.txt +++ /dev/null @@ -1,44 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(roi_cluster_fusion) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - - -find_package(ament_lint_auto REQUIRED) -find_package(ament_cmake_auto REQUIRED) -find_package(OpenCV REQUIRED) -find_package(Eigen3 REQUIRED) - -ament_auto_find_build_dependencies() - - -include_directories( - include - ${OpenCV_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} -) - -ament_auto_add_library(roi_cluster_fusion_nodelet SHARED src/roi_cluster_fusion_nodelet.cpp) -target_link_libraries(roi_cluster_fusion_nodelet - ${OpenCV_LIBRARIES} - ${EIGEN3_LIBRARIES} -) - -rclcpp_components_register_node(roi_cluster_fusion_nodelet - PLUGIN "roi_cluster_fusion::RoiClusterFusionNodelet" - EXECUTABLE roi_cluster_fusion_node -) - -ament_auto_package(INSTALL_TO_SHARE - launch -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() - endif() diff --git a/perception/roi_cluster_fusion/include/roi_cluster_fusion/roi_cluster_fusion_nodelet.hpp b/perception/roi_cluster_fusion/include/roi_cluster_fusion/roi_cluster_fusion_nodelet.hpp deleted file mode 100644 index 6b733928b3e84..0000000000000 --- a/perception/roi_cluster_fusion/include/roi_cluster_fusion/roi_cluster_fusion_nodelet.hpp +++ /dev/null @@ -1,133 +0,0 @@ -// 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 ROI_CLUSTER_FUSION__ROI_CLUSTER_FUSION_NODELET_HPP_ -#define ROI_CLUSTER_FUSION__ROI_CLUSTER_FUSION_NODELET_HPP_ - -#define EIGEN_MPL2_ONLY - -#include -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace roi_cluster_fusion -{ -class Debugger -{ -public: - explicit Debugger(rclcpp::Node * node, const int camera_num); - ~Debugger() = default; - rclcpp::Node * node_; - void showImage( - const int id, const rclcpp::Time & time, - const std::vector & image_rois, - const std::vector & pointcloud_rois, - const std::vector & points); - -private: - void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr & input_image_msg, const int id); - std::shared_ptr image_transport_; - std::vector image_subs_; - std::vector image_pubs_; - std::vector> image_buffers_; -}; - -class RoiClusterFusionNodelet : public rclcpp::Node -{ -public: - explicit RoiClusterFusionNodelet(const rclcpp::NodeOptions & options); - -private: - void fusionCallback( - tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_cluster_msg, - tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi0_msg, - tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi1_msg, - tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi2_msg, - tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi3_msg, - tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi4_msg, - tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi5_msg, - tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi6_msg, - tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi7_msg); - void cameraInfoCallback( - const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const int id); - double calcIoU( - const sensor_msgs::msg::RegionOfInterest & roi_1, - const sensor_msgs::msg::RegionOfInterest & roi_2); - double calcIoUX( - const sensor_msgs::msg::RegionOfInterest & roi_1, - const sensor_msgs::msg::RegionOfInterest & roi_2); - double calcIoUY( - const sensor_msgs::msg::RegionOfInterest & roi_1, - const sensor_msgs::msg::RegionOfInterest & roi_2); - - rclcpp::Publisher::SharedPtr - labeled_cluster_pub_; - std::vector::SharedPtr> v_camera_info_sub_; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_ptr_; - message_filters::Subscriber cluster_sub_; - std::vector>> - v_roi_sub_; - message_filters::PassThrough passthrough_; - typedef message_filters::sync_policies::ApproximateTime< - tier4_perception_msgs::msg::DetectedObjectsWithFeature, - tier4_perception_msgs::msg::DetectedObjectsWithFeature, - tier4_perception_msgs::msg::DetectedObjectsWithFeature, - tier4_perception_msgs::msg::DetectedObjectsWithFeature, - tier4_perception_msgs::msg::DetectedObjectsWithFeature, - tier4_perception_msgs::msg::DetectedObjectsWithFeature, - tier4_perception_msgs::msg::DetectedObjectsWithFeature, - tier4_perception_msgs::msg::DetectedObjectsWithFeature, - tier4_perception_msgs::msg::DetectedObjectsWithFeature> - SyncPolicy; - typedef message_filters::Synchronizer Sync; - std::shared_ptr sync_ptr_; - inline void dummyCallback( - tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input) - { - auto dummy = input; - passthrough_.add(dummy); - } - // ROS Parameters - bool use_iou_x_; - bool use_iou_y_; - bool use_iou_; - bool use_cluster_semantic_type_; - double iou_threshold_; - int rois_number_; - std::map m_camera_info_; - std::shared_ptr debugger_; -}; - -} // namespace roi_cluster_fusion - -#endif // ROI_CLUSTER_FUSION__ROI_CLUSTER_FUSION_NODELET_HPP_ diff --git a/perception/roi_cluster_fusion/src/roi_cluster_fusion_nodelet.cpp b/perception/roi_cluster_fusion/src/roi_cluster_fusion_nodelet.cpp deleted file mode 100644 index 4b2ffb50bec92..0000000000000 --- a/perception/roi_cluster_fusion/src/roi_cluster_fusion_nodelet.cpp +++ /dev/null @@ -1,511 +0,0 @@ -// 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. - -#include "roi_cluster_fusion/roi_cluster_fusion_nodelet.hpp" - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#define EIGEN_MPL2_ONLY -#include -#include - -namespace roi_cluster_fusion -{ -Debugger::Debugger(rclcpp::Node * node, const int camera_num) : node_(node) -{ - image_buffers_.resize(camera_num); - for (int id = 0; id < camera_num; ++id) { - auto sub = image_transport::create_subscription( - node, "input/image_raw" + std::to_string(id), - boost::bind(&Debugger::imageCallback, this, _1, id), "raw", rmw_qos_profile_sensor_data); - image_subs_.push_back(sub); - if (node->has_parameter("format")) { - node->undeclare_parameter("format"); - } - if (node->has_parameter("jpeg_quality")) { - node->undeclare_parameter("jpeg_quality"); - } - if (node->has_parameter("png_level")) { - node->undeclare_parameter("png_level"); - } - auto pub = image_transport::create_publisher(node, "output/image_raw" + std::to_string(id)); - image_pubs_.push_back(pub); - image_buffers_.at(id).set_capacity(5); - } -} - -void Debugger::imageCallback( - const sensor_msgs::msg::Image::ConstSharedPtr & input_image_msg, const int id) -{ - image_buffers_.at(id).push_front(input_image_msg); -} - -void Debugger::showImage( - const int id, const rclcpp::Time & time, - const std::vector & image_rois, - const std::vector & pointcloud_rois, - const std::vector & points) -{ - const boost::circular_buffer & image_buffer = - image_buffers_.at(id); - const image_transport::Publisher & image_pub = image_pubs_.at(id); - for (size_t i = 0; i < image_buffer.size(); ++i) { - if (image_buffer.at(i)->header.stamp == time) { - cv_bridge::CvImagePtr cv_ptr; - cv_ptr = cv_bridge::toCvCopy(image_buffer.at(i), image_buffer.at(i)->encoding); - for (const auto & point : points) { - cv::circle( - cv_ptr->image, cv::Point(static_cast(point.x()), static_cast(point.y())), 2, - cv::Scalar(255, 255, 255), 3, 4); - } - for (const auto & image_roi : image_rois) { - cv::line( - cv_ptr->image, cv::Point(image_roi.x_offset, image_roi.y_offset), - cv::Point(image_roi.x_offset + image_roi.width, image_roi.y_offset), - cv::Scalar(0, 0, 255), 1, cv::LINE_AA); - cv::line( - cv_ptr->image, cv::Point(image_roi.x_offset, image_roi.y_offset), - cv::Point(image_roi.x_offset, image_roi.y_offset + image_roi.height), - cv::Scalar(0, 0, 255), 1, cv::LINE_AA); - cv::line( - cv_ptr->image, cv::Point(image_roi.x_offset + image_roi.width, image_roi.y_offset), - cv::Point(image_roi.x_offset + image_roi.width, image_roi.y_offset + image_roi.height), - cv::Scalar(0, 0, 255), 1, cv::LINE_AA); - cv::line( - cv_ptr->image, cv::Point(image_roi.x_offset, image_roi.y_offset + image_roi.height), - cv::Point(image_roi.x_offset + image_roi.width, image_roi.y_offset + image_roi.height), - cv::Scalar(0, 0, 255), 1, cv::LINE_AA); - } - for (const auto & pointcloud_roi : pointcloud_rois) { - cv::line( - cv_ptr->image, cv::Point(pointcloud_roi.x_offset, pointcloud_roi.y_offset), - cv::Point(pointcloud_roi.x_offset + pointcloud_roi.width, pointcloud_roi.y_offset), - cv::Scalar(255, 0, 0), 1, cv::LINE_AA); - cv::line( - cv_ptr->image, cv::Point(pointcloud_roi.x_offset, pointcloud_roi.y_offset), - cv::Point(pointcloud_roi.x_offset, pointcloud_roi.y_offset + pointcloud_roi.height), - cv::Scalar(255, 0, 0), 1, cv::LINE_AA); - cv::line( - cv_ptr->image, - cv::Point(pointcloud_roi.x_offset + pointcloud_roi.width, pointcloud_roi.y_offset), - cv::Point( - pointcloud_roi.x_offset + pointcloud_roi.width, - pointcloud_roi.y_offset + pointcloud_roi.height), - cv::Scalar(255, 0, 0), 1, cv::LINE_AA); - cv::line( - cv_ptr->image, - cv::Point(pointcloud_roi.x_offset, pointcloud_roi.y_offset + pointcloud_roi.height), - cv::Point( - pointcloud_roi.x_offset + pointcloud_roi.width, - pointcloud_roi.y_offset + pointcloud_roi.height), - cv::Scalar(255, 0, 0), 1, cv::LINE_AA); - } - image_pub.publish(cv_ptr->toImageMsg()); - // cv::imshow("ROI" + std::to_string(id), cv_ptr->image); - // cv::waitKey(2); - - break; - } - } -} - -RoiClusterFusionNodelet::RoiClusterFusionNodelet(const rclcpp::NodeOptions & options) -: Node("roi_cluster_fusion_node", options), - tf_buffer_(this->get_clock()), - tf_listener_ptr_(tf_buffer_) -{ - use_iou_x_ = declare_parameter("use_iou_x", true); - use_iou_y_ = declare_parameter("use_iou_y", false); - use_iou_ = declare_parameter("use_iou", false); - use_cluster_semantic_type_ = declare_parameter("use_cluster_semantic_type", false); - iou_threshold_ = declare_parameter("iou_threshold", 0.1); - int rois_number = declare_parameter("rois_number", 1); - if (rois_number < 1) { - RCLCPP_WARN(this->get_logger(), "minimum roi_num is 1. current roi_num is %d", rois_number); - rois_number = 1; - } - if (8 < rois_number) { - RCLCPP_WARN(this->get_logger(), "maximum roi_num is 8. current roi_num is %d", rois_number); - rois_number = 8; - } - - cluster_sub_.subscribe(this, "clusters", rclcpp::QoS{1}.get_rmw_qos_profile()); - for (int id = 0; id < rois_number; ++id) { - std::function fcn = - std::bind(&RoiClusterFusionNodelet::cameraInfoCallback, this, std::placeholders::_1, id); - v_camera_info_sub_.push_back(this->create_subscription( - "input/camera_info" + std::to_string(id), rclcpp::QoS{1}.best_effort(), fcn)); - } - v_roi_sub_.resize(rois_number); - for (int id = 0; id < static_cast(v_roi_sub_.size()); ++id) { - v_roi_sub_.at(id) = std::make_shared< - message_filters::Subscriber>( - this, "input/rois" + std::to_string(id), rclcpp::QoS{1}.get_rmw_qos_profile()); - } - // add dummy callback to enable passthrough filter - v_roi_sub_.at(0)->registerCallback(bind(&RoiClusterFusionNodelet::dummyCallback, this, _1)); - switch (rois_number) { - case 1: - sync_ptr_ = std::make_shared( - SyncPolicy(10), cluster_sub_, *v_roi_sub_.at(0), passthrough_, passthrough_, passthrough_, - passthrough_, passthrough_, passthrough_, passthrough_); - break; - case 2: - sync_ptr_ = std::make_shared( - SyncPolicy(10), cluster_sub_, *v_roi_sub_.at(0), *v_roi_sub_.at(1), passthrough_, - passthrough_, passthrough_, passthrough_, passthrough_, passthrough_); - break; - case 3: - sync_ptr_ = std::make_shared( - SyncPolicy(10), cluster_sub_, *v_roi_sub_.at(0), *v_roi_sub_.at(1), *v_roi_sub_.at(2), - passthrough_, passthrough_, passthrough_, passthrough_, passthrough_); - break; - case 4: - sync_ptr_ = std::make_shared( - SyncPolicy(10), cluster_sub_, *v_roi_sub_.at(0), *v_roi_sub_.at(1), *v_roi_sub_.at(2), - *v_roi_sub_.at(3), passthrough_, passthrough_, passthrough_, passthrough_); - break; - case 5: - sync_ptr_ = std::make_shared( - SyncPolicy(10), cluster_sub_, *v_roi_sub_.at(0), *v_roi_sub_.at(1), *v_roi_sub_.at(2), - *v_roi_sub_.at(3), *v_roi_sub_.at(4), passthrough_, passthrough_, passthrough_); - break; - case 6: - sync_ptr_ = std::make_shared( - SyncPolicy(10), cluster_sub_, *v_roi_sub_.at(0), *v_roi_sub_.at(1), *v_roi_sub_.at(2), - *v_roi_sub_.at(3), *v_roi_sub_.at(4), *v_roi_sub_.at(5), passthrough_, passthrough_); - break; - case 7: - sync_ptr_ = std::make_shared( - SyncPolicy(10), cluster_sub_, *v_roi_sub_.at(0), *v_roi_sub_.at(1), *v_roi_sub_.at(2), - *v_roi_sub_.at(3), *v_roi_sub_.at(4), *v_roi_sub_.at(5), *v_roi_sub_.at(6), passthrough_); - break; - case 8: - sync_ptr_ = std::make_shared( - SyncPolicy(10), cluster_sub_, *v_roi_sub_.at(0), *v_roi_sub_.at(1), *v_roi_sub_.at(2), - *v_roi_sub_.at(3), *v_roi_sub_.at(4), *v_roi_sub_.at(5), *v_roi_sub_.at(6), - *v_roi_sub_.at(7)); - break; - default: - return; - } - // sync_ptr_->registerCallback( - // boost::bind(&RoiClusterFusionNodelet::fusionCallback, this, _1, _2, _3, _4, _5, _6, _7, - // _8, _9)); - sync_ptr_->registerCallback(std::bind( - &RoiClusterFusionNodelet::fusionCallback, this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, - std::placeholders::_7, std::placeholders::_8, std::placeholders::_9)); - labeled_cluster_pub_ = - this->create_publisher( - "output/labeled_clusters", rclcpp::QoS{1}); - - const bool debug_mode = declare_parameter("debug_mode", false); - if (debug_mode) { - debugger_ = std::make_shared(this, rois_number); - } -} - -void RoiClusterFusionNodelet::cameraInfoCallback( - const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const int id) -{ - m_camera_info_[id] = *input_camera_info_msg; -} - -void RoiClusterFusionNodelet::fusionCallback( - tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_cluster_msg, - tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi0_msg, - tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi1_msg, - tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi2_msg, - tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi3_msg, - tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi4_msg, - tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi5_msg, - tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi6_msg, - tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi7_msg) -{ - // Guard - if (labeled_cluster_pub_->get_subscription_count() < 1) { - return; - } - - // build output msg - tier4_perception_msgs::msg::DetectedObjectsWithFeature output_msg; - output_msg = *input_cluster_msg; - - // reset cluster semantic type - if (!use_cluster_semantic_type_) { - for (auto & feature_object : output_msg.feature_objects) { - feature_object.object.classification.front().label = - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; - feature_object.object.existence_probability = 0.0; - } - } - - // check camera info - for (int id = 0; id < static_cast(v_roi_sub_.size()); ++id) { - // debug variable - std::vector debug_image_rois; - std::vector debug_pointcloud_rois; - std::vector debug_image_points; - // cannot find camera info - if (m_camera_info_.find(id) == m_camera_info_.end()) { - RCLCPP_WARN(this->get_logger(), "no camera info. id is %d", id); - continue; - } - - // projection matrix - Eigen::Matrix4d projection; - projection << m_camera_info_.at(id).p.at(0), m_camera_info_.at(id).p.at(1), - m_camera_info_.at(id).p.at(2), m_camera_info_.at(id).p.at(3), m_camera_info_.at(id).p.at(4), - m_camera_info_.at(id).p.at(5), m_camera_info_.at(id).p.at(6), m_camera_info_.at(id).p.at(7), - m_camera_info_.at(id).p.at(8), m_camera_info_.at(id).p.at(9), m_camera_info_.at(id).p.at(10), - m_camera_info_.at(id).p.at(11); - - // get transform from cluster frame id to camera optical frame id - geometry_msgs::msg::TransformStamped transform_stamped; - try { - transform_stamped = tf_buffer_.lookupTransform( - /*target*/ m_camera_info_.at(id).header.frame_id, - /*src*/ input_cluster_msg->header.frame_id, tf2::TimePointZero); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN(this->get_logger(), "%s", ex.what()); - return; - } - - // build cluster roi - std::map m_cluster_roi; - for (size_t i = 0; i < input_cluster_msg->feature_objects.size(); ++i) { - if (input_cluster_msg->feature_objects.at(i).feature.cluster.data.empty()) { - continue; - } - - sensor_msgs::msg::PointCloud2 transformed_cluster; - tf2::doTransform( - input_cluster_msg->feature_objects.at(i).feature.cluster, transformed_cluster, - transform_stamped); - - // for reduce calc cost - // Eigen::Vector3d centroid_point; - // centroid_point << 0, 0, 0; - // for (sensor_msgs::PointCloud2ConstIterator iter_x(transformed_cluster, "x"), - // iter_y(transformed_cluster, - // "y"), iter_z(transformed_cluster, "z"); - // iter_x != iter_x.end(); - // ++iter_x, ++iter_y, ++iter_z) - // { - // centroid_point += Eigen::Vector3d(*iter_x, *iter_y, *iter_z); - // } - // centroid_point *= 1.0 / static_cast( - // input_cluster_msg->feature_objects.at(i).feature.cluster.data.size()); - // if (centroid_point.z() <= 0.0) - // continue; - - std::vector projected_points; - projected_points.reserve(transformed_cluster.data.size()); - int min_x(m_camera_info_.at(id).width), min_y(m_camera_info_.at(id).height), max_x(0), - max_y(0); - for (sensor_msgs::PointCloud2ConstIterator iter_x(transformed_cluster, "x"), - iter_y(transformed_cluster, "y"), iter_z(transformed_cluster, "z"); - iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { - if (*iter_z <= 0.0) { - continue; - } - Eigen::Vector4d projected_point = - projection * Eigen::Vector4d(*iter_x, *iter_y, *iter_z, 1.0); - Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( - projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); - if ( - 0 <= static_cast(normalized_projected_point.x()) && - static_cast(normalized_projected_point.x()) <= - static_cast(m_camera_info_.at(id).width) - 1 && - 0 <= static_cast(normalized_projected_point.y()) && - static_cast(normalized_projected_point.y()) <= - static_cast(m_camera_info_.at(id).height) - 1) { - min_x = std::min(static_cast(normalized_projected_point.x()), min_x); - min_y = std::min(static_cast(normalized_projected_point.y()), min_y); - max_x = std::max(static_cast(normalized_projected_point.x()), max_x); - max_y = std::max(static_cast(normalized_projected_point.y()), max_y); - projected_points.push_back(normalized_projected_point); - debug_image_points.push_back(normalized_projected_point); - } - } - if (projected_points.empty()) { - continue; - } - - sensor_msgs::msg::RegionOfInterest roi; - // roi.do_rectify = m_camera_info_.at(id).do_rectify; - roi.x_offset = min_x; - roi.y_offset = min_y; - roi.width = max_x - min_x; - roi.height = max_y - min_y; - m_cluster_roi.insert(std::make_pair(i, roi)); - debug_pointcloud_rois.push_back(roi); - } - - // calc iou - tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_roi_msg; - if (id == 0) { - input_roi_msg = input_roi0_msg; - } else if (id == 1) { - input_roi_msg = input_roi1_msg; - } else if (id == 2) { - input_roi_msg = input_roi2_msg; - } else if (id == 3) { - input_roi_msg = input_roi3_msg; - } else if (id == 4) { - input_roi_msg = input_roi4_msg; - } else if (id == 5) { - input_roi_msg = input_roi5_msg; - } else if (id == 6) { - input_roi_msg = input_roi6_msg; - } else if (id == 7) { - input_roi_msg = input_roi7_msg; - } - for (size_t i = 0; i < input_roi_msg->feature_objects.size(); ++i) { - int index = 0; - double max_iou = 0.0; - for (auto m_cluster_roi_itr = m_cluster_roi.begin(); m_cluster_roi_itr != m_cluster_roi.end(); - ++m_cluster_roi_itr) { - double iou(0.0), iou_x(0.0), iou_y(0.0); - if (use_iou_) { - iou = - calcIoU(m_cluster_roi_itr->second, input_roi_msg->feature_objects.at(i).feature.roi); - } - if (use_iou_x_) { - iou_x = - calcIoUX(m_cluster_roi_itr->second, input_roi_msg->feature_objects.at(i).feature.roi); - } - if (use_iou_y_) { - iou_y = - calcIoUY(m_cluster_roi_itr->second, input_roi_msg->feature_objects.at(i).feature.roi); - } - if (max_iou < iou + iou_x + iou_y) { - index = m_cluster_roi_itr->first; - max_iou = iou + iou_x + iou_y; - } - } - if ( - iou_threshold_ < max_iou && - output_msg.feature_objects.at(index).object.existence_probability <= - input_roi_msg->feature_objects.at(i).object.existence_probability && - input_roi_msg->feature_objects.at(i).object.classification.front().label != - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) { - output_msg.feature_objects.at(index).object.classification = - input_roi_msg->feature_objects.at(i).object.classification; - } - debug_image_rois.push_back(input_roi_msg->feature_objects.at(i).feature.roi); - } - if (debugger_) { - debugger_->showImage( - id, input_roi_msg->header.stamp, debug_image_rois, debug_pointcloud_rois, - debug_image_points); - } - } - // publish output msg - labeled_cluster_pub_->publish(output_msg); -} - -double RoiClusterFusionNodelet::calcIoU( - const sensor_msgs::msg::RegionOfInterest & roi_1, - const sensor_msgs::msg::RegionOfInterest & roi_2) -{ - double s_1, s_2; - s_1 = static_cast(roi_1.width * static_cast(roi_1.height)); - s_2 = static_cast(roi_2.width * static_cast(roi_2.height)); - - double overlap_s; - double overlap_max_x, overlap_max_y, overlap_min_x, overlap_min_y; - overlap_min_x = roi_1.x_offset < roi_2.x_offset ? roi_2.x_offset : roi_1.x_offset; - overlap_min_y = roi_1.y_offset < roi_2.y_offset ? roi_2.y_offset : roi_1.y_offset; - overlap_max_x = roi_1.x_offset + roi_1.width < roi_2.x_offset + roi_2.width - ? roi_1.x_offset + roi_1.width - : roi_2.x_offset + roi_2.width; - overlap_max_y = roi_1.y_offset + roi_1.height < roi_2.y_offset + roi_2.height - ? roi_1.y_offset + roi_1.height - : roi_2.y_offset + roi_2.height; - overlap_s = (overlap_max_x - overlap_min_x) * (overlap_max_y - overlap_min_y); - if (overlap_max_x < overlap_min_x || overlap_max_y < overlap_min_y) { - return 0.0; - } - return overlap_s / (s_1 + s_2 - overlap_s); -} -double RoiClusterFusionNodelet::calcIoUX( - const sensor_msgs::msg::RegionOfInterest & roi_1, - const sensor_msgs::msg::RegionOfInterest & roi_2) -{ - double s_1, s_2; - s_1 = static_cast(roi_1.width); - s_2 = static_cast(roi_2.width); - double overlap_s; - double overlap_max_x, overlap_max_y, overlap_min_x, overlap_min_y; - overlap_min_x = roi_1.x_offset < roi_2.x_offset ? roi_2.x_offset : roi_1.x_offset; - overlap_min_y = roi_1.y_offset < roi_2.y_offset ? roi_2.y_offset : roi_1.y_offset; - overlap_max_x = roi_1.x_offset + roi_1.width < roi_2.x_offset + roi_2.width - ? roi_1.x_offset + roi_1.width - : roi_2.x_offset + roi_2.width; - overlap_max_y = roi_1.y_offset + roi_1.height < roi_2.y_offset + roi_2.height - ? roi_1.y_offset + roi_1.height - : roi_2.y_offset + roi_2.height; - overlap_s = (overlap_max_x - overlap_min_x); - if (overlap_max_x < overlap_min_x || overlap_max_y < overlap_min_y) { - return 0.0; - } - return overlap_s / (s_1 + s_2 - overlap_s); -} -double RoiClusterFusionNodelet::calcIoUY( - const sensor_msgs::msg::RegionOfInterest & roi_1, - const sensor_msgs::msg::RegionOfInterest & roi_2) -{ - double s_1, s_2; - s_1 = static_cast(roi_1.height); - s_2 = static_cast(roi_2.height); - double overlap_s; - double overlap_max_x, overlap_max_y, overlap_min_x, overlap_min_y; - overlap_min_x = roi_1.x_offset < roi_2.x_offset ? roi_2.x_offset : roi_1.x_offset; - overlap_min_y = roi_1.y_offset < roi_2.y_offset ? roi_2.y_offset : roi_1.y_offset; - overlap_max_x = roi_1.x_offset + roi_1.width < roi_2.x_offset + roi_2.width - ? roi_1.x_offset + roi_1.width - : roi_2.x_offset + roi_2.width; - overlap_max_y = roi_1.y_offset + roi_1.height < roi_2.y_offset + roi_2.height - ? roi_1.y_offset + roi_1.height - : roi_2.y_offset + roi_2.height; - overlap_s = (overlap_max_y - overlap_min_y); - if (overlap_max_x < overlap_min_x || overlap_max_y < overlap_min_y) { - return 0.0; - } - return overlap_s / (s_1 + s_2 - overlap_s); -} -} // namespace roi_cluster_fusion - -RCLCPP_COMPONENTS_REGISTER_NODE(roi_cluster_fusion::RoiClusterFusionNodelet) diff --git a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md index 8dc72a22e8903..bd251550c07e7 100644 --- a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md +++ b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md @@ -438,14 +438,6 @@ TODO ## Future extensions / Unimplemented parts -- **Design of the limit distance of the avoidance width** - - - Currently, the avoidance width limit is given as a fixed value as a parameter for both left and right. Therefore, even if there is a wall, this module generates an avoidance path towards the wall. It is needed to calculate the area that the vehicle can move and generate an avoidance path within the area. - -- **Dynamic calculation of avoidance width** - - - The length of the avoidance shift needs to be adjusted according to the situation. For example, the length of the margin between the ego and the target object can be large if the another lane is clear and safe. In another case, it may be small if there are other vehicles in the oncoming lane. The ability to set such an appropriate avoidance margin has not yet been implemented, and currently the largest margin is always used for the avoidance. - - **Planning on the intersection** - If it is known that the ego vehicle is going to stop in the middle of avoidance execution (for example, at a red traffic light), sometimes the avoidance should not be executed until the vehicle is ready to move. This is because it is impossible to predict how the environment will change during the stop.　 This is especially important at intersections. 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 e5c1b89454f4e..5d6a374f970eb 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 @@ -42,6 +42,7 @@ #include #include #include +#include #include #include @@ -68,6 +69,7 @@ using nav_msgs::msg::Odometry; using route_handler::RouteHandler; using tier4_planning_msgs::msg::PathChangeModule; using tier4_planning_msgs::msg::PathChangeModuleArray; +using tier4_planning_msgs::msg::Scenario; using visualization_msgs::msg::MarkerArray; class BehaviorPathPlannerNode : public rclcpp::Node @@ -79,6 +81,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr route_subscriber_; rclcpp::Subscription::SharedPtr vector_map_subscriber_; rclcpp::Subscription::SharedPtr velocity_subscriber_; + rclcpp::Subscription::SharedPtr scenario_subscriber_; rclcpp::Subscription::SharedPtr perception_subscriber_; rclcpp::Subscription::SharedPtr external_approval_subscriber_; rclcpp::Subscription::SharedPtr force_approval_subscriber_; @@ -94,6 +97,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node std::shared_ptr planner_data_; std::shared_ptr bt_manager_; tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; + Scenario::SharedPtr current_scenario_{nullptr}; std::string prev_ready_module_name_ = "NONE"; diff --git a/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml b/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml index 63f9ba16292a5..22d5283b1b0e3 100644 --- a/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml +++ b/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml @@ -11,6 +11,7 @@ + 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 bd392853bcb23..57bf8e364c9af 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -51,6 +51,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onVelocity, this, _1)); perception_subscriber_ = create_subscription( "~/input/perception", 1, std::bind(&BehaviorPathPlannerNode::onPerception, this, _1)); + scenario_subscriber_ = create_subscription( + "~/input/scenario", 1, [this](const Scenario::SharedPtr msg) { current_scenario_ = msg; }); external_approval_subscriber_ = create_subscription( "~/input/external_approval", 1, std::bind(&BehaviorPathPlannerNode::onExternalApproval, this, _1)); @@ -58,11 +60,12 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "~/input/force_approval", 1, std::bind(&BehaviorPathPlannerNode::onForceApproval, this, _1)); // route_handler + auto qos_transient_local = rclcpp::QoS{1}.transient_local(); vector_map_subscriber_ = create_subscription( - "~/input/vector_map", rclcpp::QoS{1}.transient_local(), + "~/input/vector_map", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onMap, this, _1)); route_subscriber_ = create_subscription( - "~/input/route", 1, std::bind(&BehaviorPathPlannerNode::onRoute, this, _1)); + "~/input/route", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onRoute, this, _1)); // publisher path_publisher_ = create_publisher("~/output/path", 1); @@ -445,8 +448,15 @@ BehaviorTreeManagerParam BehaviorPathPlannerNode::getBehaviorTreeManagerParam() void BehaviorPathPlannerNode::waitForData() { // wait until mandatory data is ready + while (!current_scenario_ && rclcpp::ok()) { + RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for scenario topic"); + rclcpp::spin_some(this->get_node_base_interface()); + rclcpp::Rate(100).sleep(); + } + while (!planner_data_->route_handler->isHandlerReady() && rclcpp::ok()) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for route to be ready"); + RCLCPP_INFO_SKIPFIRST_THROTTLE( + get_logger(), *get_clock(), 5000, "waiting for route to be ready"); rclcpp::spin_some(this->get_node_base_interface()); rclcpp::Rate(100).sleep(); } @@ -455,7 +465,7 @@ void BehaviorPathPlannerNode::waitForData() if (planner_data_->dynamic_object && planner_data_->self_odometry) { break; } - RCLCPP_WARN_THROTTLE( + RCLCPP_INFO_SKIPFIRST_THROTTLE( get_logger(), *get_clock(), 5000, "waiting for vehicle pose, vehicle_velocity, and obstacles"); rclcpp::spin_some(this->get_node_base_interface()); @@ -470,6 +480,11 @@ void BehaviorPathPlannerNode::run() { RCLCPP_DEBUG(get_logger(), "----- BehaviorPathPlannerNode start -----"); + // behavior_path_planner runs only in LANE DRIVING scenario. + if (current_scenario_->current_scenario != Scenario::LANEDRIVING) { + return; + } + // update planner data updateCurrentPose(); diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml index 98c72567709bd..298d93447fde2 100644 --- a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml +++ b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml @@ -1,10 +1,11 @@ /**: ros__parameters: occlusion_spot: - detection_method: "predicted_object" # [-] candidate is "occupancy_grid" or "predicted_object" + detection_method: "occupancy_grid" # [-] candidate is "occupancy_grid" or "predicted_object" pass_judge: "smooth_velocity" # [-] candidate is "smooth_velocity" or "current_velocity" filter_occupancy_grid: true # [-] whether to filter occupancy grid by morphologyEx or not use_object_info: true # [-] whether to reflect object info to occupancy grid map or not + use_moving_object_ray_cast: true # [-] whether to reflect moving object ray_cast to occupancy grid map or not use_partition_lanelet: true # [-] whether to use partition lanelet map data pedestrian_vel: 1.5 # [m/s] assume pedestrian is dashing from occlusion at this velocity pedestrian_radius: 0.3 # [m] assume pedestrian width(0.2m) + margin(0.1m) @@ -18,8 +19,8 @@ lateral_distance: 1.5 # [m] maximum lateral distance to consider hidden collision motion: safety_ratio: 0.8 # [-] jerk/acceleration ratio for safety - max_slow_down_jerk: -0.7 # [m/s^3] minimum jerk deceleration for safe brake. - max_slow_down_accel: -2.0 # [m/s^2] minimum accel deceleration for safe brake. + max_slow_down_jerk: -0.5 # [m/s^3] minimum jerk deceleration for safe brake. + max_slow_down_accel: -1.8 # [m/s^2] minimum accel deceleration for safe brake. non_effective_jerk: -0.3 # [m/s^3] weak jerk for velocity planning. non_effective_acceleration: -1.0 # [m/s^2] weak deceleration for velocity planning. min_allowed_velocity: 1.0 # [m/s] minimum velocity allowed diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/raycast_shadow.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/raycast_shadow.drawio.svg new file mode 100644 index 0000000000000..7f34eb7c032c2 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/raycast_shadow.drawio.svg @@ -0,0 +1,385 @@ + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ ego +
+
+
+ + + ego + + + + + + + +
+
+
+ stuck vehicle +
+
+
+ + + stuck ve... + + + + + + + +
+
+
+ cruising +
+ vehicle +
+
+
+ + + cruising... + + + + + + + + + + + + +
+
+
+ possible collision +
+
+
+ + + possi... + + + + + + + + +
+
+
+ not possible collision +
+
+
+ + + not p... + + + + + + + + + +
+
+
+ occupancy grid consider possible collision object ray cast +
+
+
+ + + occupancy grid consider possible collision object ray c... + + + + + + + + + + + + +
+
+
+ + passable +
+ collision +
+ +
+
+
+ + + passable... + + + + + + + + + + + + +
+
+
+ stuck vehicle +
+
+
+ + + stuck ve... + + + + + + + +
+
+
+ + passable + +
+
+
+ + + passable + + + + + + + +
+
+
+ cruising +
+ vehicle +
+
+
+ + + cruising... + + + + + + + + +
+
+
+ passable collision +
+
+
+ + + passa... + + + + + + + + +
+
+
+ passable collision +
+
+
+ + + passa... + + + + + + + + + Viewer does not support full SVG 1.1 + + + + \ No newline at end of file diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp index 5eb18a7ecde8d..52594094bf8b9 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp @@ -20,38 +20,108 @@ #include #include #include +#include +#include #include #include #include #include #include +#include +#include +#include +#include +#include #include #include #include #include +#include +#include #include +namespace tf2 +{ +inline void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped & out) +{ + out.stamp_ = tf2_ros::fromMsg(msg.header.stamp); + out.frame_id_ = msg.header.frame_id; + tf2::Transform tmp; + fromMsg(msg.pose, tmp); + out.setData(tmp); +} + +// Remove after this commit is released +// https://github.com/ros2/geometry2/commit/e9da371d81e388a589540357c050e262442f1b4a +inline geometry_msgs::msg::Point & toMsg(const tf2::Vector3 & in, geometry_msgs::msg::Point & out) +{ + out.x = in.getX(); + out.y = in.getY(); + out.z = in.getZ(); + return out; +} + +// Remove after this commit is released +// https://github.com/ros2/geometry2/commit/e9da371d81e388a589540357c050e262442f1b4a +inline void fromMsg(const geometry_msgs::msg::Point & in, tf2::Vector3 & out) +{ + out = tf2::Vector3(in.x, in.y, in.z); +} + +template <> +inline void doTransform( + const geometry_msgs::msg::Point & t_in, geometry_msgs::msg::Point & t_out, + const geometry_msgs::msg::TransformStamped & transform) +{ + tf2::Transform t; + fromMsg(transform.transform, t); + tf2::Vector3 v_in; + fromMsg(t_in, v_in); + tf2::Vector3 v_out = t * v_in; + toMsg(v_out, t_out); +} +} // namespace tf2 + namespace behavior_velocity_planner { +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PoseStamped; namespace grid_utils { using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::TransformStamped; +using nav_msgs::msg::MapMetaData; using nav_msgs::msg::OccupancyGrid; namespace occlusion_cost_value { static constexpr unsigned char FREE_SPACE = 0; static constexpr unsigned char UNKNOWN = 50; static constexpr unsigned char OCCUPIED = 100; -static constexpr unsigned char FREE_SPACE_IMAGE = 0; static constexpr unsigned char UNKNOWN_IMAGE = 128; static constexpr unsigned char OCCUPIED_IMAGE = 255; } // namespace occlusion_cost_value +struct PolarCoordinates +{ + double radius; + double theta; +}; + +inline PolarCoordinates toPolarCoordinates(const Point2d & origin, const Point2d & point) +{ + const double dy = point.y() - origin.y(); + const double dx = point.x() - origin.x(); + const double radius = std::hypot(dy, dx); + const double theta = std::atan2(dy, dx); + return {radius, theta}; +} + struct GridParam { int free_space_max; // maximum value of a freespace cell in the occupancy grid @@ -87,14 +157,26 @@ bool isCollisionFree( void getCornerPositions( std::vector & corner_positions, const grid_map::GridMap & grid, const OcclusionSpotSquare & occlusion_spot_square); +boost::optional generateOccupiedPolygon( + const Polygon2d & occupancy_poly, const Polygons2d & stuck_vehicle_foot_prints, + const Polygons2d & moving_vehicle_foot_prints, const Point & position); +//!< @brief generate occupied polygon from foot print +void generateOccupiedImage( + const OccupancyGrid & occ_grid, cv::Mat & inout_image, + const Polygons2d & stuck_vehicle_foot_prints, const Polygons2d & moving_vehicle_foot_prints, + const bool use_object_foot_print, const bool use_object_raycast); +cv::Point toCVPoint( + const Point & geom_point, const double width_m, const double height_m, const double resolution); void imageToOccupancyGrid(const cv::Mat & cv_image, nav_msgs::msg::OccupancyGrid * occupancy_grid); void toQuantizedImage( const nav_msgs::msg::OccupancyGrid & occupancy_grid, cv::Mat * cv_image, const GridParam & param); void denoiseOccupancyGridCV( - const nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid_ptr, + const OccupancyGrid::ConstSharedPtr occupancy_grid_ptr, + const Polygons2d & stuck_vehicle_foot_prints, const Polygons2d & moving_vehicle_foot_prints, grid_map::GridMap & grid_map, const GridParam & param, const bool is_show_debug_window, - const bool filter_occupancy_grid); -void addObjectsToGridMap(const PredictedObjects & objs, grid_map::GridMap & grid); + const bool filter_occupancy_grid, const bool use_object_footprints, + const bool use_object_ray_casts); +void addObjectsToGridMap(const std::vector & objs, grid_map::GridMap & grid); } // namespace grid_utils } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp index 356212191c7c5..525c1ae06861e 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp @@ -113,12 +113,13 @@ struct PlannerParam { DETECTION_METHOD detection_method; PASS_JUDGE pass_judge; - bool is_show_occlusion; // [-] - bool is_show_cv_window; // [-] - bool is_show_processing_time; // [-] - bool filter_occupancy_grid; // [-] - bool use_object_info; // [-] - bool use_partition_lanelet; // [-] + bool is_show_occlusion; // [-] + bool is_show_cv_window; // [-] + bool is_show_processing_time; // [-] + bool filter_occupancy_grid; // [-] + bool use_object_info; // [-] + bool use_moving_object_ray_cast; // [-] + bool use_partition_lanelet; // [-] // parameters in yaml double detection_area_length; // [m] double detection_area_max_length; // [m] @@ -214,12 +215,16 @@ lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path); void handleCollisionOffset(std::vector & possible_collisions, double offset); void clipPathByLength( const PathWithLaneId & path, PathWithLaneId & clipped, const double max_length = 100.0); -bool isStuckVehicle(PredictedObject obj, const double min_vel); -std::vector filterDynamicObjectByDetectionArea( - std::vector & objs, const Polygons2d & polys); -std::vector getParkedVehicles( - const PredictedObjects & dyn_objects, const PlannerParam & param, - std::vector & debug_point); +//!< @brief extract target vehicles +bool isStuckVehicle(const PredictedObject & obj, const double min_vel); +bool isMovingVehicle(const PredictedObject & obj, const double min_vel); +std::vector extractVehicles(const PredictedObjects::ConstSharedPtr objects_ptr); +std::vector filterVehiclesByDetectionArea( + const std::vector & objs, const Polygons2d & polys); +bool isVehicle(const ObjectClassification & obj_class); +void categorizeVehicles( + const std::vector & vehicles, Polygons2d & stuck_vehicle_foot_prints, + Polygons2d & moving_vehicle_foot_prints, const double stuck_vehicle_vel); bool generatePossibleCollisionsFromObjects( std::vector & possible_collisions, const PathWithLaneId & path, const PlannerParam & param, const double offset_from_start_to_ego, @@ -231,11 +236,6 @@ void calculateCollisionPathPointFromOcclusionSpot( PossibleCollisionInfo & pc, const lanelet::BasicPoint2d & obstacle_point, const double offset_from_ego_to_target, const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param); -//!< @brief create hidden collision behind parked car -void createPossibleCollisionBehindParkedVehicle( - std::vector & possible_collisions, const PathWithLaneId & path, - const PlannerParam & param, const double offset_from_ego_to_target, - const PredictedObjects::ConstSharedPtr & dyn_obj_arr); //!< @brief set velocity and orientation to collision point based on previous Path with laneId void calcSlowDownPointsForPossibleCollision( const int closest_idx, const PathWithLaneId & path, const double offset, diff --git a/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp index c38a30429219f..1364dfbd3b84f 100644 --- a/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp @@ -101,7 +101,7 @@ class TrafficLightModule : public SceneModuleInterface bool isPassthrough(const double & signed_arc_length) const; - bool hasTrafficLightColor( + bool hasTrafficLightCircleColor( const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color) const; diff --git a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp index 89b955c47c346..761a5e8c1f987 100644 --- a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp @@ -142,18 +142,29 @@ inline bool smoothPath( 0, trajectory.size(), external_v_limit->max_velocity, trajectory); } const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); - // calc ego internal division point on path - const auto traj_with_ego_point_with_idx = - getLerpTrajectoryPointWithIdx(*traj_lateral_acc_filtered, current_pose.position); + auto nearest_idx = + tier4_autoware_utils::findNearestIndex(*traj_lateral_acc_filtered, current_pose.position); + const auto dist_to_nearest = tier4_autoware_utils::calcSignedArcLength( + *traj_lateral_acc_filtered, current_pose.position, nearest_idx); + + // if trajectory has the almost same point as ego, don't insert the ego point + constexpr double epsilon = 1e-2; TrajectoryPoints traj_with_ego_point_on_path = *traj_lateral_acc_filtered; - TrajectoryPoint ego_point_on_path = traj_with_ego_point_with_idx.first; - const size_t nearest_seg_idx = traj_with_ego_point_with_idx.second; - //! insert ego projected pose on path so new nearest segment will be nearest_seg_idx + 1 - traj_with_ego_point_on_path.insert( - traj_with_ego_point_on_path.begin() + nearest_seg_idx, ego_point_on_path); + if (std::fabs(dist_to_nearest) > epsilon) { + // calc ego internal division point on path + const auto traj_with_ego_point_with_idx = + getLerpTrajectoryPointWithIdx(*traj_lateral_acc_filtered, current_pose.position); + TrajectoryPoint ego_point_on_path = traj_with_ego_point_with_idx.first; + const size_t nearest_seg_idx = traj_with_ego_point_with_idx.second; + //! insert ego projected pose on path so new nearest segment will be nearest_seg_idx + 1 + traj_with_ego_point_on_path.insert( + traj_with_ego_point_on_path.begin() + nearest_seg_idx, ego_point_on_path); + + // ego point inserted is new nearest point + nearest_idx = traj_with_ego_point_with_idx.second + 1; + } // Resample trajectory with ego-velocity based interval distances - auto traj_resampled = - smoother->resampleTrajectory(traj_with_ego_point_on_path, v0, nearest_seg_idx + 1); + auto traj_resampled = smoother->resampleTrajectory(traj_with_ego_point_on_path, v0, nearest_idx); const auto traj_resampled_closest = findNearestIndex(*traj_resampled, current_pose, max, M_PI_4); std::vector debug_trajectories; // Clip trajectory from closest point diff --git a/planning/behavior_velocity_planner/occlusion-spot-design.md b/planning/behavior_velocity_planner/occlusion-spot-design.md index a2bc68f2b0cf2..ba7ccb7dd5b1d 100644 --- a/planning/behavior_velocity_planner/occlusion-spot-design.md +++ b/planning/behavior_velocity_planner/occlusion-spot-design.md @@ -8,19 +8,17 @@ This module plans safe velocity to slow down before reaching collision point tha ### Activation Timing -This module is activated when the ego-lane has a private/public attribute. +This module is activated if `launch_occlusion_spot` becomes true ### Limitation -To solve the excessive deceleration due to false positive of the perception, the logic of collision spot is switched according to the road type (public/private). This point has not been discussed in detail and needs to be improved (see the description below). +To solve the excessive deceleration due to false positive of the perception, the logic of detection method can be selectable. This point has not been discussed in detail and needs to be improved (see the description below). ### Inner-workings / Algorithms -#### Logics Working In Private/Public Load +#### Logics Working -Deceleration for the occlusion spot works on **different logic** depending on whether the driving road is **public or private**. - -There are several types of occlusions, such as "occlusions generated by parked vehicles" and "occlusions caused by obstructions". In situations such as driving on **private roads**, where people jump out of the way frequently, all possible occlusion spots must be taken into account. Therefore, on private roads, the deceleration plan considers all occlusion spots calculated from the **occupancy grid**. On the other hand, while driving at high speeds, it is not reasonable to take into account all occlusion spots (e.g., jumping out from behind a guardrail). Therefore, on **public roads**, the target of the deceleration calculation is currently limited to only the occlusion spots generated by vehicles parked on the road shoulder, which uses the **dynamic object** information. +There are several types of occlusions, such as "occlusions generated by parked vehicles" and "occlusions caused by obstructions". In situations such as driving on **road with obstacles**, where people jump out of the way frequently, all possible occlusion spots must be taken into account. This module considers all occlusion spots calculated from the **occupancy grid**, but it is not reasonable to take into account all occlusion spots for example, people jumping out from behind a guardrail, or behind cruising vehicle. Therefore currently detection area will be limited to to use **dynamic object** information. Note that this decision logic is still under development and needs to be improved. @@ -31,7 +29,7 @@ This module inserts safe velocity at the collision point estimated from the asso This module consider 3 policies that are very important for risk predicting system for occlusion spot. -1. "Passable" without deceleration (Not implemented yet) +1. "Passable" without deceleration If ego vehicle speed is high enough to pass the occlusion spot and expected to have no collision with any objects coming out of the occlusion spot, then it's possible for ego vehicle to pass the spot without deceleration. 2. "Predictable" with enough distance to occlusion @@ -100,6 +98,11 @@ use object info to make occupancy grid more accurate obstacle that can run out from occlusion should have free space until intersection from ego vehicle ![brief](./docs/occlusion_spot/collision_free.drawio.svg) +##### Possible Collision + +obstacle that can run out from occlusion is interruped by moving vehicle. +![brief](./docs/occlusion_spot/raycast_shadow.drawio.svg) + #### Module Parameters | Parameter | Type | Description | diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 9e2e86ccfe421..4395302d9092a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -123,8 +123,8 @@ bool IntersectionModule::modifyPathVelocity( return false; } - if (stop_line_idx <= 0 || pass_judge_line_idx <= 0) { - RCLCPP_DEBUG(logger_, "stop line or pass judge line is at path[0], ignore planning."); + if (stop_line_idx <= 0) { + RCLCPP_DEBUG(logger_, "stop line line is at path[0], ignore planning."); RCLCPP_DEBUG(logger_, "===== plan end ====="); return true; } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp index b78ee53e1f53a..91927807e70ba 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp @@ -56,19 +56,13 @@ std::vector> pointsToRays( return lines; } -void addObjectsToGridMap(const PredictedObjects & objs, grid_map::GridMap & grid) +void addObjectsToGridMap(const std::vector & objs, grid_map::GridMap & grid) { auto & grid_data = grid["layer"]; - for (const auto & obj : objs.objects) { + for (const auto & obj : objs) { Polygon2d foot_print_polygon = planning_utils::toFootprintPolygon(obj); grid_map::Polygon grid_polygon; const auto & pos = obj.kinematics.initial_pose_with_covariance.pose.position; - const auto & obj_label = obj.classification.at(0).label; - using autoware_auto_perception_msgs::msg::ObjectClassification; - if ( - obj_label != ObjectClassification::CAR || obj_label != ObjectClassification::TRUCK || - obj_label != ObjectClassification::BUS || obj_label != ObjectClassification::TRAILER) - continue; if (grid.isInside(grid_map::Position(pos.x, pos.y))) continue; try { for (const auto & point : foot_print_polygon.outer()) { @@ -155,13 +149,8 @@ void findOcclusionSpots( OcclusionSpotSquare occlusion_spot_square; if (isOcclusionSpotSquare( occlusion_spot_square, grid_data, *iterator, min_occlusion_spot_size, grid.getSize())) { - if (!grid.getPosition(occlusion_spot_square.index, occlusion_spot_square.position)) { - continue; - } - std::vector corner_positions; - getCornerPositions(corner_positions, grid, occlusion_spot_square); - for (const grid_map::Position & corner : corner_positions) { - occlusion_spot_positions.emplace_back(corner); + if (grid.getPosition(occlusion_spot_square.index, occlusion_spot_square.position)) { + occlusion_spot_positions.emplace_back(occlusion_spot_square.position); } } } @@ -247,6 +236,199 @@ void getCornerPositions( corner_positions.emplace_back(corner_position); } } + +boost::optional generateOcclusionPolygon( + const Polygon2d & occupancy_poly, const Point2d & origin, const Point2d & min_theta_pos, + const Point2d & max_theta_pos, const double ray_max_length = 100.0) +{ + using tier4_autoware_utils::normalizeRadian; + const double origin_x = origin.x(); + const double origin_y = origin.y(); + // TODO(tanaka): consider this later + const double delay_angle = M_PI / 6.0; + const double min_theta = + normalizeRadian(std::atan2(min_theta_pos.y() - origin_y, min_theta_pos.x() - origin_x), 0.0) - + delay_angle; + const double max_theta = + normalizeRadian(std::atan2(max_theta_pos.y() - origin_y, max_theta_pos.x() - origin_x), 0.0) + + delay_angle; + LineString2d theta_min_ray = { + origin, + {origin_x + ray_max_length * std::cos(min_theta), + origin_y + ray_max_length * std::sin(min_theta)}}; + LineString2d theta_max_ray = { + origin, + {origin_x + ray_max_length * std::cos(max_theta), + origin_y + ray_max_length * std::sin(max_theta)}}; + Polygon2d occlusion_poly; + occlusion_poly.outer() = {min_theta_pos, max_theta_pos}; + std::vector min_intersections; + std::vector max_intersections; + boost::geometry::intersection(occupancy_poly, theta_min_ray, min_intersections); + boost::geometry::intersection(occupancy_poly, theta_max_ray, max_intersections); + if (!min_intersections.empty()) { + // has min theta intersection + occlusion_poly.outer().emplace_back(min_intersections.front()); + } + if (!max_intersections.empty()) { + // has max theta intersection + occlusion_poly.outer().emplace_back(max_intersections.front()); + } + //! case outside detection area + if (occlusion_poly.outer().size() == 2) return boost::none; + boost::geometry::correct(occlusion_poly); + Polygon2d hull_poly; + boost::geometry::convex_hull(occlusion_poly, hull_poly); + return hull_poly; +} + +Polygon2d generateOccupancyPolygon(const nav_msgs::msg::MapMetaData & info, const double r = 100) +{ + using tier4_autoware_utils::calcOffsetPose; + // generate occupancy polygon from grid origin + Polygon2d poly; // create counter clockwise poly + poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, 0, 0, 0).position)); + poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, r, 0, 0).position)); + poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, r, r, 0).position)); + poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, 0, r, 0).position)); + return poly; +} + +std::pair calcEdgePoint(const Polygon2d & foot_print, const Point2d & origin) +{ + size_t min_idx = 0; + size_t max_idx = 0; + double min_theta = std::numeric_limits::max(); + double max_theta = -std::numeric_limits::max(); + for (size_t i = 0; i < foot_print.outer().size(); i++) { + const auto & f = foot_print.outer().at(i); + PolarCoordinates polar = toPolarCoordinates(origin, f); + const double theta_norm = tier4_autoware_utils::normalizeRadian(polar.theta, 0.0); + if (theta_norm < min_theta) { + min_theta = theta_norm; + min_idx = i; + } + if (theta_norm > max_theta) { + max_theta = theta_norm; + max_idx = i; + } + } + return std::make_pair(min_idx, max_idx); +} + +boost::optional generateOccupiedPolygon( + const Polygon2d & occupancy_poly, const Polygon2d & foot_print, const Point & position) +{ + Point2d origin = {position.x, position.y}; + const auto & edge_pair = calcEdgePoint(foot_print, origin); + const size_t min_idx = edge_pair.first; + const size_t max_idx = edge_pair.second; + Polygon2d occupied_polygon; + const auto & poly = generateOcclusionPolygon( + occupancy_poly, origin, foot_print.outer().at(min_idx), foot_print.outer().at(max_idx)); + return poly; +} + +Point transformFromMap2Grid(const TransformStamped & geom_tf_map2grid, const Point2d & p) +{ + Point geom_pt = tier4_autoware_utils::createPoint(p.x(), p.y(), 0); + Point transformed_geom_pt; + // from map coordinate to occupancy grid corrdinate + tf2::doTransform(geom_pt, transformed_geom_pt, geom_tf_map2grid); + return transformed_geom_pt; +} + +void generateOccupiedImage( + const OccupancyGrid & occ_grid, cv::Mat & inout_image, + const Polygons2d & stuck_vehicle_foot_prints, const Polygons2d & moving_vehicle_foot_prints, + const bool use_object_foot_print, const bool use_object_raycast) +{ + const auto & occ = occ_grid; + OccupancyGrid occupancy_grid; + PoseStamped grid_origin; + const double width = occ.info.width * occ.info.resolution; + const double height = occ.info.height * occ.info.resolution; + Point scan_origin = occ.info.origin.position; + scan_origin.x += 0.5 * width; + scan_origin.y += 0.5 * height; + + // calculate grid origin + { + grid_origin.header = occ.header; + grid_origin.pose.position.x = occ.info.origin.position.x; + grid_origin.pose.position.y = occ.info.origin.position.y; + grid_origin.pose.position.z = 0.0; // same z as foot print polygon + } + + // header + { + occupancy_grid.header.stamp = occ.header.stamp; + occupancy_grid.header.frame_id = "map"; + } + + // info + { + occupancy_grid.info.map_load_time = occ.header.stamp; + occupancy_grid.info.resolution = occ.info.resolution; + occupancy_grid.info.width = occ.info.width; + occupancy_grid.info.height = occ.info.height; + occupancy_grid.info.origin = grid_origin.pose; + } + + constexpr uint8_t occupied_space = occlusion_cost_value::OCCUPIED_IMAGE; + // get transform + tf2::Stamped tf_grid2map; + tf2::Stamped tf_map2grid; + tf2::fromMsg(grid_origin, tf_grid2map); + tf_map2grid.setData(tf_grid2map.inverse()); + const auto geom_tf_map2grid = tf2::toMsg(tf_map2grid); + + // create not Detection Area using opencv + std::vector> cv_polygons; + std::vector cv_polygon; + Polygon2d occupancy_poly = generateOccupancyPolygon(occupancy_grid.info); + if (use_object_raycast) { + for (const auto & foot_print : moving_vehicle_foot_prints) { + // calculate occlusion polygon from moving vehicle + const auto polys = generateOccupiedPolygon(occupancy_poly, foot_print, scan_origin); + if (polys == boost::none) continue; + // transform to cv point and stuck it to cv polygon + for (const auto & p : polys.get().outer()) { + const Point transformed_geom_pt = transformFromMap2Grid(geom_tf_map2grid, p); + cv_polygon.emplace_back( + toCVPoint(transformed_geom_pt, width, height, occupancy_grid.info.resolution)); + } + cv_polygons.push_back(cv_polygon); + // clear previously addeed points + cv_polygon.clear(); + } + } + if (use_object_foot_print) { + for (const auto & foot_print : stuck_vehicle_foot_prints) { + for (const auto & p : foot_print.outer()) { + const Point transformed_geom_pt = transformFromMap2Grid(geom_tf_map2grid, p); + cv_polygon.emplace_back( + toCVPoint(transformed_geom_pt, width, height, occupancy_grid.info.resolution)); + } + cv_polygons.push_back(cv_polygon); + // clear previously addeed points + cv_polygon.clear(); + } + } + for (const auto & p : cv_polygons) { + // fill in occlusion area and copy to occupancy grid + cv::fillConvexPoly(inout_image, p, cv::Scalar(occupied_space)); + } +} + +cv::Point toCVPoint( + const Point & geom_point, const double width_m, const double height_m, const double resolution) +{ + return cv::Point( + static_cast((height_m - geom_point.y) / resolution), + static_cast((width_m - geom_point.x) / resolution)); +} + void imageToOccupancyGrid(const cv::Mat & cv_image, nav_msgs::msg::OccupancyGrid * occupancy_grid) { const int width = cv_image.cols; @@ -257,7 +439,7 @@ void imageToOccupancyGrid(const cv::Mat & cv_image, nav_msgs::msg::OccupancyGrid for (int y = height - 1; y >= 0; y--) { const int idx = (height - 1 - y) + (width - 1 - x) * height; unsigned char intensity = cv_image.at(y, x); - if (intensity == grid_utils::occlusion_cost_value::FREE_SPACE_IMAGE) { + if (intensity == grid_utils::occlusion_cost_value::FREE_SPACE) { intensity = grid_utils::occlusion_cost_value::FREE_SPACE; } else if (intensity == grid_utils::occlusion_cost_value::UNKNOWN_IMAGE) { intensity = grid_utils::occlusion_cost_value::UNKNOWN; @@ -280,7 +462,7 @@ void toQuantizedImage( const int idx = (height - 1 - y) + (width - 1 - x) * height; unsigned char intensity = occupancy_grid.data.at(idx); if (intensity <= param.free_space_max) { - intensity = grid_utils::occlusion_cost_value::FREE_SPACE_IMAGE; + intensity = grid_utils::occlusion_cost_value::FREE_SPACE; } else if (param.free_space_max < intensity && intensity < param.occupied_min) { intensity = grid_utils::occlusion_cost_value::UNKNOWN_IMAGE; } else if (param.occupied_min <= intensity) { @@ -294,24 +476,46 @@ void toQuantizedImage( } void denoiseOccupancyGridCV( - const nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid_ptr, + const OccupancyGrid::ConstSharedPtr occupancy_grid_ptr, + const Polygons2d & stuck_vehicle_foot_prints, const Polygons2d & moving_vehicle_foot_prints, grid_map::GridMap & grid_map, const GridParam & param, const bool is_show_debug_window, - const bool filter_occupancy_grid) + const bool filter_occupancy_grid, const bool use_object_footprints, + const bool use_object_ray_casts) { OccupancyGrid occupancy_grid = *occupancy_grid_ptr; cv::Mat cv_image( occupancy_grid.info.width, occupancy_grid.info.height, CV_8UC1, cv::Scalar(grid_utils::occlusion_cost_value::OCCUPIED)); toQuantizedImage(occupancy_grid, &cv_image, param); - constexpr int num_iter = 2; + + //! show orignal occupancy grid to compare difference + if (is_show_debug_window) { + cv::namedWindow("original", cv::WINDOW_NORMAL); + cv::imshow("original", cv_image); + cv::waitKey(1); + } + + //! raycast object shadow using vehicle + if (use_object_footprints || use_object_ray_casts) { + generateOccupiedImage( + occupancy_grid, cv_image, stuck_vehicle_foot_prints, moving_vehicle_foot_prints, + use_object_footprints, use_object_ray_casts); + if (is_show_debug_window) { + cv::namedWindow("object ray shadow", cv::WINDOW_NORMAL); + cv::imshow("object ray shadow", cv_image); + cv::waitKey(1); + } + } + //!< @brief opening & closing to remove noise in occupancy grid if (filter_occupancy_grid) { + constexpr int num_iter = 2; cv::morphologyEx(cv_image, cv_image, cv::MORPH_CLOSE, cv::Mat(), cv::Point(-1, -1), num_iter); - } - if (is_show_debug_window) { - cv::namedWindow("morph", cv::WINDOW_NORMAL); - cv::imshow("morph", cv_image); - cv::waitKey(1); + if (is_show_debug_window) { + cv::namedWindow("morph", cv::WINDOW_NORMAL); + cv::imshow("morph", cv_image); + cv::waitKey(1); + } } imageToOccupancyGrid(cv_image, &occupancy_grid); grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp index 4eb5635461c7e..aacd7ff01aa2b 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp @@ -60,6 +60,7 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) } pp.filter_occupancy_grid = node.declare_parameter(ns + ".filter_occupancy_grid", false); pp.use_object_info = node.declare_parameter(ns + ".use_object_info", false); + pp.use_moving_object_ray_cast = node.declare_parameter(ns + ".use_moving_object_ray_cast", false); pp.use_partition_lanelet = node.declare_parameter(ns + ".use_partition_lanelet", false); pp.pedestrian_vel = node.declare_parameter(ns + ".pedestrian_vel", 1.5); pp.pedestrian_radius = node.declare_parameter(ns + ".pedestrian_radius", 0.5); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp index 55f7047654749..97afa55478ef3 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp @@ -192,39 +192,53 @@ void clipPathByLength( } } -bool isStuckVehicle(PredictedObject obj, const double min_vel) +bool isVehicle(const PredictedObject & obj) { - if ( - obj.classification.at(0).label == ObjectClassification::CAR || - obj.classification.at(0).label == ObjectClassification::TRUCK || - obj.classification.at(0).label == ObjectClassification::BUS) { - if (std::abs(obj.kinematics.initial_twist_with_covariance.twist.linear.x) < min_vel) { - return true; + const auto & label = obj.classification.at(0).label; + return ( + label == ObjectClassification::CAR || label == ObjectClassification::TRUCK || + label == ObjectClassification::BUS || label == ObjectClassification::TRAILER); +} + +bool isStuckVehicle(const PredictedObject & obj, const double min_vel) +{ + if (!isVehicle(obj)) return false; + const auto & obj_vel = obj.kinematics.initial_twist_with_covariance.twist.linear.x; + return std::abs(obj_vel) <= min_vel; +} + +bool isMovingVehicle(const PredictedObject & obj, const double min_vel) +{ + if (!isVehicle(obj)) return false; + const auto & obj_vel = obj.kinematics.initial_twist_with_covariance.twist.linear.x; + return std::abs(obj_vel) > min_vel; +} + +std::vector extractVehicles(const PredictedObjects::ConstSharedPtr objects_ptr) +{ + std::vector vehicles; + for (const auto & obj : objects_ptr->objects) { + if (occlusion_spot_utils::isVehicle(obj)) { + vehicles.emplace_back(obj); } } - return false; + return vehicles; } -std::vector getParkedVehicles( - const PredictedObjects & dyn_objects, const PlannerParam & param, - std::vector & debug_point) +void categorizeVehicles( + const std::vector & vehicles, Polygons2d & stuck_vehicle_foot_prints, + Polygons2d & moving_vehicle_foot_prints, const double stuck_vehicle_vel) { - std::vector parked_vehicles; - std::vector points; - for (const auto & obj : dyn_objects.objects) { - bool is_parked_vehicle = true; - if (!occlusion_spot_utils::isStuckVehicle(obj, param.stuck_vehicle_vel)) { - continue; - } - const geometry_msgs::msg::Point & p = obj.kinematics.initial_pose_with_covariance.pose.position; - BasicPoint2d obj_point(p.x, p.y); - if (is_parked_vehicle) { - parked_vehicles.emplace_back(obj); - points.emplace_back(p); + moving_vehicle_foot_prints.clear(); + stuck_vehicle_foot_prints.clear(); + for (const auto & vehicle : vehicles) { + if (isMovingVehicle(vehicle, stuck_vehicle_vel)) { + moving_vehicle_foot_prints.emplace_back(planning_utils::toFootprintPolygon(vehicle)); + } else if (isStuckVehicle(vehicle, stuck_vehicle_vel)) { + stuck_vehicle_foot_prints.emplace_back(planning_utils::toFootprintPolygon(vehicle)); } } - debug_point = points; - return parked_vehicles; + return; } ArcCoordinates getOcclusionPoint(const PredictedObject & obj, const ConstLineString2d & ll_string) @@ -343,8 +357,8 @@ bool generatePossibleCollisionsFromObjects( return !possible_collisions.empty(); } -std::vector filterDynamicObjectByDetectionArea( - std::vector & objs, const Polygons2d & polys) +std::vector filterVehiclesByDetectionArea( + const std::vector & objs, const Polygons2d & polys) { std::vector filtered_obj; // stuck points by predicted objects @@ -438,16 +452,16 @@ boost::optional generateOneNotableCollisionFromOcclusionS PossibleCollisionInfo pc = calculateCollisionPathPointFromOcclusionSpot( arc_coord_occlusion_point, arc_coord_collision_point, path_lanelet, param); const auto & ip = pc.intersection_pose.position; - bool collision_free_at_intersection = grid_utils::isCollisionFree( - grid, occlusion_spot_position, grid_map::Position(ip.x, ip.y), param.pedestrian_radius); bool is_obstacle_blocked_by_partition = false; - if (!collision_free_at_intersection) continue; if (param.use_partition_lanelet) { const auto & op = obstacle_point; const LineString2d obstacle_vec = {{op[0], op[1]}, {ip.x, ip.y}}; is_obstacle_blocked_by_partition = isBlockedByPartition(obstacle_vec, partition_lanelets); } if (is_obstacle_blocked_by_partition) continue; + bool collision_free_at_intersection = grid_utils::isCollisionFree( + grid, occlusion_spot_position, grid_map::Position(ip.x, ip.y), param.pedestrian_radius); + if (!collision_free_at_intersection) continue; distance_lower_bound = dist; candidate = pc; has_collision = true; diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index 9aa70942fd7db..1bf89bcd848a1 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -36,6 +36,23 @@ RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, time_msg); \ } +namespace +{ +namespace utils = behavior_velocity_planner::occlusion_spot_utils; +using autoware_auto_perception_msgs::msg::PredictedObject; +std::vector extractStuckVehicle( + const std::vector & vehicles, const double stop_velocity) +{ + std::vector stuck_vehicles; + for (const auto & obj : vehicles) { + if (utils::isStuckVehicle(obj, stop_velocity)) { + stuck_vehicles.emplace_back(obj); + } + } + return stuck_vehicles; +} +} // namespace + namespace behavior_velocity_planner { namespace utils = occlusion_spot_utils; @@ -75,7 +92,8 @@ bool OcclusionSpotModule::modifyPathVelocity( param_.v.max_stop_accel = planner_data_->max_stop_acceleration_threshold; param_.v.v_ego = planner_data_->current_velocity->twist.linear.x; param_.v.a_ego = planner_data_->current_accel.get(); - param_.v.delay_time = planner_data_->delay_response_time; + // introduce delay ratio until system delay param will introduce + param_.v.delay_time = 0.5; const double detection_area_offset = 5.0; // for visualization and stability param_.detection_area_max_length = planning_utils::calcJudgeLineDistWithJerkLimit( @@ -118,16 +136,25 @@ bool OcclusionSpotModule::modifyPathVelocity( ego_pose.position, partition_lanelets_, debug_data_.close_partition); } DEBUG_PRINT(show_time, "extract[ms]: ", stop_watch_.toc("processing_time", true)); - std::vector parked_vehicle_point; + const auto objects_ptr = planner_data_->predicted_objects; + const auto vehicles = utils::extractVehicles(objects_ptr); + const std::vector filtered_vehicles = + utils::filterVehiclesByDetectionArea(vehicles, debug_data_.detection_area_polygons); + DEBUG_PRINT(show_time, "filter obj[ms]: ", stop_watch_.toc("processing_time", true)); if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { const auto & occ_grid_ptr = planner_data_->occupancy_grid; if (!occ_grid_ptr) return true; // no data grid_map::GridMap grid_map; + Polygons2d stuck_vehicle_foot_prints; + Polygons2d moving_vehicle_foot_prints; + utils::categorizeVehicles( + filtered_vehicles, stuck_vehicle_foot_prints, moving_vehicle_foot_prints, + param_.stuck_vehicle_vel); + // occ -> image grid_utils::denoiseOccupancyGridCV( - occ_grid_ptr, grid_map, param_.grid, param_.is_show_cv_window, param_.filter_occupancy_grid); - if (param_.use_object_info) { - grid_utils::addObjectsToGridMap(*planner_data_->predicted_objects, grid_map); - } + occ_grid_ptr, stuck_vehicle_foot_prints, moving_vehicle_foot_prints, grid_map, param_.grid, + param_.is_show_cv_window, param_.filter_occupancy_grid, param_.use_object_info, + param_.use_moving_object_ray_cast); DEBUG_PRINT(show_time, "grid [ms]: ", stop_watch_.toc("processing_time", true)); // Note: Don't consider offset from path start to ego here if (!utils::generatePossibleCollisionsFromGridMap( @@ -137,15 +164,10 @@ bool OcclusionSpotModule::modifyPathVelocity( return true; } } else if (param_.detection_method == utils::DETECTION_METHOD::PREDICTED_OBJECT) { - const auto & dynamic_obj_arr_ptr = planner_data_->predicted_objects; - if (!dynamic_obj_arr_ptr) return true; // no data - std::vector obj = - utils::getParkedVehicles(*dynamic_obj_arr_ptr, param_, parked_vehicle_point); - const auto filtered_obj = - utils::filterDynamicObjectByDetectionArea(obj, debug_data_.detection_area_polygons); + const auto stuck_vehicles = extractStuckVehicle(filtered_vehicles, param_.stuck_vehicle_vel); // Note: Don't consider offset from path start to ego here if (!utils::generatePossibleCollisionsFromObjects( - possible_collisions, interp_path, param_, offset_from_start_to_ego, filtered_obj)) { + possible_collisions, interp_path, param_, offset_from_start_to_ego, stuck_vehicles)) { // no occlusion spot return true; } @@ -158,7 +180,6 @@ bool OcclusionSpotModule::modifyPathVelocity( // apply safe velocity using ebs and pbs deceleration utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_); // these debug topics needs computation resource - debug_data_.parked_vehicle_point = parked_vehicle_point; debug_data_.z = path->points.front().point.pose.position.z; debug_data_.possible_collisions = possible_collisions; debug_data_.interp_path = interp_path; diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp index e509b80b3464a..ac8bc9729b585 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp @@ -364,7 +364,8 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const bool TrafficLightModule::isTrafficSignalStop( const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state) const { - if (hasTrafficLightColor(tl_state, autoware_auto_perception_msgs::msg::TrafficLight::GREEN)) { + if (hasTrafficLightCircleColor( + tl_state, autoware_auto_perception_msgs::msg::TrafficLight::GREEN)) { return false; } @@ -465,6 +466,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopP target_point_with_lane_id.point.pose.position.x = target_point.x(); target_point_with_lane_id.point.pose.position.y = target_point.y(); target_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; + debug_data_.stop_poses.push_back(target_point_with_lane_id.point.pose); // Insert stop pose into path or replace with zero velocity size_t insert_index = insert_target_point_idx; @@ -484,13 +486,16 @@ autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopP return modified_path; } -bool TrafficLightModule::hasTrafficLightColor( +bool TrafficLightModule::hasTrafficLightCircleColor( const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color) const { - const auto it_lamp = std::find_if( - tl_state.lights.begin(), tl_state.lights.end(), - [&lamp_color](const auto & x) { return x.color == lamp_color; }); + using autoware_auto_perception_msgs::msg::TrafficLight; + + const auto it_lamp = + std::find_if(tl_state.lights.begin(), tl_state.lights.end(), [&lamp_color](const auto & x) { + return x.shape == TrafficLight::CIRCLE && x.color == lamp_color; + }); return it_lamp != tl_state.lights.end(); } diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp index 478e069539cbe..e3ad37c1ef87f 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -233,7 +233,8 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti // Subscribers { route_sub_ = create_subscription( - "~/input/route", 1, std::bind(&FreespacePlannerNode::onRoute, this, _1)); + "~/input/route", rclcpp::QoS{1}.transient_local(), + std::bind(&FreespacePlannerNode::onRoute, this, _1)); occupancy_grid_sub_ = create_subscription( "~/input/occupancy_grid", 1, std::bind(&FreespacePlannerNode::onOccupancyGrid, this, _1)); scenario_sub_ = create_subscription( diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index 6f38883aa4790..d93cc890054a5 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -51,6 +51,10 @@ template double lerpTwistX( const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx) { + if (points.size() == 1) { + return points.at(0).longitudinal_velocity_mps; + } + constexpr double epsilon = 1e-6; const double closest_to_target_dist = @@ -70,6 +74,10 @@ template double lerpPoseZ( const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx) { + if (points.size() == 1) { + return points.at(0).pose.position.z; + } + constexpr double epsilon = 1e-6; const double closest_to_target_dist = 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 96264dce277fe..f9927114fc35c 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 @@ -383,7 +383,7 @@ void SimplePlanningSimulator::add_measurement_noise( odom.pose.pose.position.x += (*n.pos_dist_)(*n.rand_engine_); odom.pose.pose.position.y += (*n.pos_dist_)(*n.rand_engine_); const auto velocity_noise = (*n.vel_dist_)(*n.rand_engine_); - odom.twist.twist.linear.x = velocity_noise; + odom.twist.twist.linear.x += velocity_noise; float32_t yaw = motion::motion_common::to_angle(odom.pose.pose.orientation); yaw += static_cast((*n.rpy_dist_)(*n.rand_engine_)); odom.pose.pose.orientation = motion::motion_common::from_angle(yaw); diff --git a/system/emergency_handler/README.md b/system/emergency_handler/README.md index 253f53a07c3d5..530a27a2204c3 100644 --- a/system/emergency_handler/README.md +++ b/system/emergency_handler/README.md @@ -8,7 +8,7 @@ Emergency Handler is a node to select proper MRM from from system failure state ### State Transitions -![fail-safe-state](https://tier4.github.io/autoware.proj/tree/main/design/apis/image/fail-safe-state.drawio.svg) +![fail-safe-state](image/fail-safe-state.drawio.svg) ## Inputs / Outputs diff --git a/system/emergency_handler/image/fail-safe-state.drawio.svg b/system/emergency_handler/image/fail-safe-state.drawio.svg new file mode 100644 index 0000000000000..1b2b5df2eaa23 --- /dev/null +++ b/system/emergency_handler/image/fail-safe-state.drawio.svg @@ -0,0 +1,238 @@ + + + + + + + + + + + + +
+
+
+ + Normal + +
+
+
+ + + Normal + + + + + + + + + + + Emergency + + + + + + +
+
+
+ OverrideRequesting +
+
+
+ + + OverrideRequesting + + + + + + + + + + +
+
+
+ MrmOperating +
+
+
+ + + MrmOperating + + + + + + + + + + + +
+
+
+ MrmFailed +
+
+
+ + + MrmFailed + + + + + + + + + +
+
+
+ MrmSucceeded +
+
+
+ + + MrmSucceeded + + + + + + + + +
+
+
+ no reaction +
+
+
+ + + no reaction + + + + + + +
+
+
+ failure +
+
+
+ + + failure + + + + + + +
+
+
+ warning +
+
+
+ + + warning + + + + + + +
+
+
+ emergency +
+
+
+ + + emergency + + + + + + +
+
+
+ recovery +
+
+
+ + + recovery + + + + + + +
+
+
+ success +
+
+
+ + + success + + + + + + +
+
+
+ fatal +
+
+
+ + + fatal + + + + + + + + + Viewer does not support full SVG 1.1 + + + + \ No newline at end of file