diff --git a/.github/workflows/spell-check-differential.yaml b/.github/workflows/spell-check-differential.yaml deleted file mode 100644 index 1fbf2ff46925c..0000000000000 --- a/.github/workflows/spell-check-differential.yaml +++ /dev/null @@ -1,16 +0,0 @@ -name: spell-check-differential - -on: - pull_request: - -jobs: - spell-check-differential: - runs-on: ubuntu-latest - steps: - - name: Check out repository - uses: actions/checkout@v4 - - - name: Run spell-check - uses: autowarefoundation/autoware-github-actions/spell-check@v1 - with: - cspell-json-url: https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/.cspell.json diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml index c1ad3e3df140e..2033239824d95 100644 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ b/common/autoware_auto_perception_rviz_plugin/package.xml @@ -8,7 +8,8 @@ Satoshi Tanaka Taiki Tanaka Takeshi Miura - + Shunsuke Miura + Yoshi Ri Apache 2.0 ament_cmake diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/CMakeLists.txt b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/CMakeLists.txt new file mode 100644 index 0000000000000..da67a6f63aeae --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/CMakeLists.txt @@ -0,0 +1,140 @@ +cmake_minimum_required(VERSION 3.8) +project(awf_2d_overlay_vehicle) + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(autoware_auto_vehicle_msgs REQUIRED) +find_package(tier4_planning_msgs REQUIRED) +find_package(autoware_perception_msgs REQUIRED) +ament_auto_find_build_dependencies() + +find_package(rviz_2d_overlay_msgs REQUIRED) + +find_package(rviz_common REQUIRED) +find_package(rviz_rendering REQUIRED) +find_package(rviz_ogre_vendor REQUIRED) +find_package(std_msgs REQUIRED) + +set( + headers_to_moc + include/overlay_text_display.hpp + include/signal_display.hpp + +) + +set( + headers_to_not_moc + include/steering_wheel_display.hpp + include/gear_display.hpp + include/speed_display.hpp + include/turn_signals_display.hpp + include/traffic_display.hpp + include/speed_limit_display.hpp +) + + + +foreach(header "${headers_to_moc}") + qt5_wrap_cpp(display_moc_files "${header}") +endforeach() + +set( + display_source_files + src/overlay_text_display.cpp + src/overlay_utils.cpp + src/signal_display.cpp + src/steering_wheel_display.cpp + src/gear_display.cpp + src/speed_display.cpp + src/turn_signals_display.cpp + src/traffic_display.cpp + src/speed_limit_display.cpp + +) + +add_library( + ${PROJECT_NAME} SHARED + ${display_moc_files} + ${headers_to_not_moc} + ${display_source_files} +) + +set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 17) +target_compile_options(${PROJECT_NAME} PRIVATE -Wall -Wextra -Wpedantic) + +target_include_directories( + ${PROJECT_NAME} PUBLIC + $ + $ + ${Qt5Widgets_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} +) + +target_link_libraries( + ${PROJECT_NAME} PUBLIC + rviz_ogre_vendor::OgreMain + rviz_ogre_vendor::OgreOverlay +) + + +target_compile_definitions(${PROJECT_NAME} PRIVATE "${PROJECT_NAME}_BUILDING_LIBRARY") + +# prevent pluginlib from using boost +target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) + +ament_target_dependencies( + ${PROJECT_NAME} + PUBLIC + rviz_common + rviz_rendering + rviz_2d_overlay_msgs + autoware_auto_vehicle_msgs + tier4_planning_msgs + autoware_perception_msgs +) + +ament_export_include_directories(include) +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + rviz_common + rviz_ogre_vendor +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS + DESTINATION lib/${PROJECT_NAME} +) + +# Copy the assets directory to the installation directory +install( + DIRECTORY assets/ + DESTINATION share/${PROJECT_NAME}/assets +) + +add_definitions(-DQT_NO_KEYWORDS) + +ament_package( + CONFIG_EXTRAS "awf_2d_overlay_vehicle-extras.cmake" +) diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/LICENSE b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/LICENSE new file mode 100644 index 0000000000000..a2fe2d3d1c7ed --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/LICENSE @@ -0,0 +1,12 @@ +Copyright (c) 2022, Team Spatzenhirn +Copyright (c) 2014, JSK Lab + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/README.md b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/README.md new file mode 100644 index 0000000000000..6728f26878e10 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/README.md @@ -0,0 +1,54 @@ +# awf_2d_overlay_vehicle + +Plugin for displaying 2D overlays over the RViz2 3D scene. + +Based on the [jsk_visualization](https://github.com/jsk-ros-pkg/jsk_visualization) +package, under the 3-Clause BSD license. + +## Purpose + +This plugin provides a visual and easy-to-understand display of vehicle speed, turn signal, steering status and gears. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ---------------------------------------- | ------------------------------------------------------- | ---------------------------------- | +| `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | The topic is vehicle twist | +| `/vehicle/status/turn_indicators_status` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | +| `/vehicle/status/hazard_status` | `autoware_auto_vehicle_msgs::msg::HazardReport` | The topic is status of hazard | +| `/vehicle/status/steering_status` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | +| `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs::msg::GearReport` | The topic is status of gear | + +## Parameter + +### Core Parameters + +#### SignalDisplay + +| Name | Type | Default Value | Description | +| ------------------------ | ------ | -------------------- | --------------------------------- | +| `property_width_` | int | 128 | Width of the plotter window [px] | +| `property_height_` | int | 128 | Height of the plotter window [px] | +| `property_left_` | int | 128 | Left of the plotter window [px] | +| `property_top_` | int | 128 | Top of the plotter window [px] | +| `property_signal_color_` | QColor | QColor(25, 255, 240) | Turn Signal color | + +## Assumptions / Known limits + +TBD. + +## Usage + +1. Start rviz and select Add under the Displays panel. + + ![select_add](./assets/images/select_add.png) + +2. Select any one of the tier4_vehicle_rviz_plugin and press OK. + + ![select_vehicle_plugin](./assets/images/select_vehicle_plugin.png) + +3. Enter the name of the topic where you want to view the status. + + ![select_topic_name](./assets/images/select_topic_name.png) diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/LICENSE b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/LICENSE new file mode 100644 index 0000000000000..cc04d393573f0 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/LICENSE @@ -0,0 +1,93 @@ +Copyright 2011 The Quicksand Project Authors (https://github.com/andrew-paglinawan/QuicksandFamily), with Reserved Font Name “Quicksand”. + +This Font Software is licensed under the SIL Open Font License, Version 1.1. +This license is copied below, and is also available with a FAQ at: +http://scripts.sil.org/OFL + + +----------------------------------------------------------- +SIL OPEN FONT LICENSE Version 1.1 - 26 February 2007 +----------------------------------------------------------- + +PREAMBLE +The goals of the Open Font License (OFL) are to stimulate worldwide +development of collaborative font projects, to support the font creation +efforts of academic and linguistic communities, and to provide a free and +open framework in which fonts may be shared and improved in partnership +with others. + +The OFL allows the licensed fonts to be used, studied, modified and +redistributed freely as long as they are not sold by themselves. The +fonts, including any derivative works, can be bundled, embedded, +redistributed and/or sold with any software provided that any reserved +names are not used by derivative works. The fonts and derivatives, +however, cannot be released under any other type of license. The +requirement for fonts to remain under this license does not apply +to any document created using the fonts or their derivatives. + +DEFINITIONS +"Font Software" refers to the set of files released by the Copyright +Holder(s) under this license and clearly marked as such. This may +include source files, build scripts and documentation. + +"Reserved Font Name" refers to any names specified as such after the +copyright statement(s). + +"Original Version" refers to the collection of Font Software components as +distributed by the Copyright Holder(s). + +"Modified Version" refers to any derivative made by adding to, deleting, +or substituting -- in part or in whole -- any of the components of the +Original Version, by changing formats or by porting the Font Software to a +new environment. + +"Author" refers to any designer, engineer, programmer, technical +writer or other person who contributed to the Font Software. + +PERMISSION & CONDITIONS +Permission is hereby granted, free of charge, to any person obtaining +a copy of the Font Software, to use, study, copy, merge, embed, modify, +redistribute, and sell modified and unmodified copies of the Font +Software, subject to the following conditions: + +1) Neither the Font Software nor any of its individual components, +in Original or Modified Versions, may be sold by itself. + +2) Original or Modified Versions of the Font Software may be bundled, +redistributed and/or sold with any software, provided that each copy +contains the above copyright notice and this license. These can be +included either as stand-alone text files, human-readable headers or +in the appropriate machine-readable metadata fields within text or +binary files as long as those fields can be easily viewed by the user. + +3) No Modified Version of the Font Software may use the Reserved Font +Name(s) unless explicit written permission is granted by the corresponding +Copyright Holder. This restriction only applies to the primary font name as +presented to the users. + +4) The name(s) of the Copyright Holder(s) or the Author(s) of the Font +Software shall not be used to promote, endorse or advertise any +Modified Version, except to acknowledge the contribution(s) of the +Copyright Holder(s) and the Author(s) or with their explicit written +permission. + +5) The Font Software, modified or unmodified, in part or in whole, +must be distributed entirely under this license, and must not be +distributed under any other license. The requirement for fonts to +remain under this license does not apply to any document created +using the Font Software. + +TERMINATION +This license becomes null and void if any of the above conditions are +not met. + +DISCLAIMER +THE FONT SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO ANY WARRANTIES OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT +OF COPYRIGHT, PATENT, TRADEMARK, OR OTHER RIGHT. IN NO EVENT SHALL THE +COPYRIGHT HOLDER BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, +INCLUDING ANY GENERAL, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL +DAMAGES, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +FROM, OUT OF THE USE OR INABILITY TO USE THE FONT SOFTWARE OR FROM +OTHER DEALINGS IN THE FONT SOFTWARE. diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Bold.ttf b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Bold.ttf new file mode 100644 index 0000000000000..07d5127c04b17 Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Bold.ttf differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Light.ttf b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Light.ttf new file mode 100644 index 0000000000000..800531084fa6c Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Light.ttf differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Medium.ttf b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Medium.ttf new file mode 100644 index 0000000000000..f4634cd7c3f16 Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Medium.ttf differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Regular.ttf b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Regular.ttf new file mode 100644 index 0000000000000..60323ed6abdf1 Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-Regular.ttf differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-SemiBold.ttf b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-SemiBold.ttf new file mode 100644 index 0000000000000..52059c3a3da3f Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/font/Quicksand/static/Quicksand-SemiBold.ttf differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/arrow.png b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/arrow.png new file mode 100644 index 0000000000000..18de535ce4489 Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/arrow.png differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_add.png b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_add.png new file mode 100644 index 0000000000000..c5130b6092384 Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_add.png differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_topic_name.png b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_topic_name.png new file mode 100644 index 0000000000000..5466bcf0050df Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_topic_name.png differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_vehicle_plugin.png b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_vehicle_plugin.png new file mode 100644 index 0000000000000..863f63d728616 Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/select_vehicle_plugin.png differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/traffic.png b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/traffic.png new file mode 100644 index 0000000000000..960985a3da5de Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/traffic.png differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/wheel.png b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/wheel.png new file mode 100644 index 0000000000000..07b1e33132e43 Binary files /dev/null and b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/assets/images/wheel.png differ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/awf_2d_overlay_vehicle-extras.cmake b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/awf_2d_overlay_vehicle-extras.cmake new file mode 100644 index 0000000000000..9d6f156555e57 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/awf_2d_overlay_vehicle-extras.cmake @@ -0,0 +1,31 @@ +# Copyright (c) 2021, Open Source Robotics Foundation, Inc. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# find package Qt5 because otherwise using the rviz_default_plugins::rviz_default_plugins +# exported target will complain that the Qt5::Widgets target does not exist +find_package(Qt5 REQUIRED QUIET COMPONENTS Widgets) diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/gear_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/gear_display.hpp new file mode 100644 index 0000000000000..b289bfc541cee --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/gear_display.hpp @@ -0,0 +1,49 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GEAR_DISPLAY_HPP_ +#define GEAR_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp" + +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +class GearDisplay +{ +public: + GearDisplay(); + void drawGearIndicator(QPainter & painter, const QRectF & backgroundRect); + void updateGearData(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); + +private: + int current_gear_; // Internal variable to store current gear + QColor gray = QColor(194, 194, 194); +}; + +} // namespace awf_2d_overlay_vehicle + +#endif // GEAR_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_text_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_text_display.hpp new file mode 100644 index 0000000000000..9b3ecccf6301c --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_text_display.hpp @@ -0,0 +1,157 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// -*- mode: c++; -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Team Spatzenhirn + * Copyright (c) 2014, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +#ifndef OVERLAY_TEXT_DISPLAY_HPP_ +#define OVERLAY_TEXT_DISPLAY_HPP_ + +#include "rviz_2d_overlay_msgs/msg/overlay_text.hpp" +#ifndef Q_MOC_RUN +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#endif + +namespace awf_2d_overlay_vehicle +{ +class OverlayTextDisplay +: public rviz_common::RosTopicDisplay +{ + Q_OBJECT +public: + OverlayTextDisplay(); + virtual ~OverlayTextDisplay(); + +protected: + awf_2d_overlay_vehicle::OverlayObject::SharedPtr overlay_; + + int texture_width_; + int texture_height_; + + bool overtake_fg_color_properties_; + bool overtake_bg_color_properties_; + bool overtake_position_properties_; + bool align_bottom_; + bool invert_shadow_; + QColor bg_color_; + QColor fg_color_; + int text_size_; + int line_width_; + std::string text_; + QStringList font_families_; + std::string font_; + int horizontal_dist_; + int vertical_dist_; + HorizontalAlignment horizontal_alignment_; + VerticalAlignment vertical_alignment_; + + void onInitialize() override; + void onEnable() override; + void onDisable() override; + void update(float wall_dt, float ros_dt) override; + void reset() override; + + bool require_update_texture_; + // properties are raw pointers since they are owned by Qt + rviz_common::properties::BoolProperty * overtake_position_properties_property_; + rviz_common::properties::BoolProperty * overtake_fg_color_properties_property_; + rviz_common::properties::BoolProperty * overtake_bg_color_properties_property_; + rviz_common::properties::BoolProperty * align_bottom_property_; + rviz_common::properties::BoolProperty * invert_shadow_property_; + rviz_common::properties::IntProperty * hor_dist_property_; + rviz_common::properties::IntProperty * ver_dist_property_; + rviz_common::properties::EnumProperty * hor_alignment_property_; + rviz_common::properties::EnumProperty * ver_alignment_property_; + rviz_common::properties::IntProperty * width_property_; + rviz_common::properties::IntProperty * height_property_; + rviz_common::properties::IntProperty * text_size_property_; + rviz_common::properties::IntProperty * line_width_property_; + rviz_common::properties::ColorProperty * bg_color_property_; + rviz_common::properties::FloatProperty * bg_alpha_property_; + rviz_common::properties::ColorProperty * fg_color_property_; + rviz_common::properties::FloatProperty * fg_alpha_property_; + rviz_common::properties::EnumProperty * font_property_; + +protected Q_SLOTS: + void updateOvertakePositionProperties(); + void updateOvertakeFGColorProperties(); + void updateOvertakeBGColorProperties(); + void updateAlignBottom(); + void updateInvertShadow(); + void updateHorizontalDistance(); + void updateVerticalDistance(); + void updateHorizontalAlignment(); + void updateVerticalAlignment(); + void updateWidth(); + void updateHeight(); + void updateTextSize(); + void updateFGColor(); + void updateFGAlpha(); + void updateBGColor(); + void updateBGAlpha(); + void updateFont(); + void updateLineWidth(); + +private: + void processMessage(rviz_2d_overlay_msgs::msg::OverlayText::ConstSharedPtr msg) override; +}; +} // namespace awf_2d_overlay_vehicle + +#endif // OVERLAY_TEXT_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_utils.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_utils.hpp new file mode 100644 index 0000000000000..1270f15ca80ae --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/overlay_utils.hpp @@ -0,0 +1,141 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Team Spatzenhirn + * Copyright (c) 2014, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef OVERLAY_UTILS_HPP_ +#define OVERLAY_UTILS_HPP_ + +#include +#include + +#include "rviz_2d_overlay_msgs/msg/overlay_text.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace awf_2d_overlay_vehicle +{ +class OverlayObject; + +class ScopedPixelBuffer +{ +public: + explicit ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer); + virtual ~ScopedPixelBuffer(); + virtual Ogre::HardwarePixelBufferSharedPtr getPixelBuffer(); + virtual QImage getQImage(unsigned int width, unsigned int height); + virtual QImage getQImage(OverlayObject & overlay); + virtual QImage getQImage(unsigned int width, unsigned int height, QColor & bg_color); + virtual QImage getQImage(OverlayObject & overlay, QColor & bg_color); + +protected: + Ogre::HardwarePixelBufferSharedPtr pixel_buffer_; +}; + +enum class VerticalAlignment : uint8_t { + CENTER = rviz_2d_overlay_msgs::msg::OverlayText::CENTER, + TOP = rviz_2d_overlay_msgs::msg::OverlayText::TOP, + BOTTOM = rviz_2d_overlay_msgs::msg::OverlayText::BOTTOM, +}; + +enum class HorizontalAlignment : uint8_t { + LEFT = rviz_2d_overlay_msgs::msg::OverlayText::LEFT, + RIGHT = rviz_2d_overlay_msgs::msg::OverlayText::RIGHT, + CENTER = rviz_2d_overlay_msgs::msg::OverlayText::CENTER +}; + +/** + * Helper class for realizing an overlay object on top of the rviz 3D panel. + * + * This class is supposed to be instantiated in the onInitialize method of the + * rviz_common::Display class. + */ +class OverlayObject +{ +public: + using SharedPtr = std::shared_ptr; + + explicit OverlayObject(const std::string & name); + virtual ~OverlayObject(); + + virtual std::string getName() const; + virtual void hide(); + virtual void show(); + virtual bool isTextureReady() const; + virtual void updateTextureSize(unsigned int width, unsigned int height); + virtual ScopedPixelBuffer getBuffer(); + virtual void setPosition( + double hor_dist, double ver_dist, HorizontalAlignment hor_alignment = HorizontalAlignment::LEFT, + VerticalAlignment ver_alignment = VerticalAlignment::TOP); + virtual void setDimensions(double width, double height); + virtual bool isVisible() const; + virtual unsigned int getTextureWidth() const; + virtual unsigned int getTextureHeight() const; + +protected: + const std::string name_; + Ogre::Overlay * overlay_; + Ogre::PanelOverlayElement * panel_; + Ogre::MaterialPtr panel_material_; + Ogre::TexturePtr texture_; +}; +} // namespace awf_2d_overlay_vehicle + +#endif // OVERLAY_UTILS_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/signal_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/signal_display.hpp new file mode 100644 index 0000000000000..aff475aba2f33 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/signal_display.hpp @@ -0,0 +1,124 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SIGNAL_DISPLAY_HPP_ +#define SIGNAL_DISPLAY_HPP_ +#ifndef Q_MOC_RUN +#include "gear_display.hpp" +#include "overlay_utils.hpp" +#include "speed_display.hpp" +#include "speed_limit_display.hpp" +#include "steering_wheel_display.hpp" +#include "traffic_display.hpp" +#include "turn_signals_display.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#endif + +namespace awf_2d_overlay_vehicle +{ +class SignalDisplay : public rviz_common::Display +{ + Q_OBJECT +public: + SignalDisplay(); + ~SignalDisplay() override; + +protected: + void onInitialize() override; + void update(float wall_dt, float ros_dt) override; + void reset() override; + void onEnable() override; + void onDisable() override; + +private Q_SLOTS: + void updateOverlaySize(); + void updateSmallOverlaySize(); + void updateOverlayPosition(); + void updateOverlayColor(); + void topic_updated_gear(); + void topic_updated_steering(); + void topic_updated_speed(); + void topic_updated_speed_limit(); + void topic_updated_turn_signals(); + void topic_updated_hazard_lights(); + void topic_updated_traffic(); + +private: + std::mutex mutex_; + awf_2d_overlay_vehicle::OverlayObject::SharedPtr overlay_; + rviz_common::properties::IntProperty * property_width_; + rviz_common::properties::IntProperty * property_height_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::ColorProperty * property_signal_color_; + std::unique_ptr steering_topic_property_; + std::unique_ptr gear_topic_property_; + std::unique_ptr speed_topic_property_; + std::unique_ptr turn_signals_topic_property_; + std::unique_ptr hazard_lights_topic_property_; + std::unique_ptr traffic_topic_property_; + std::unique_ptr speed_limit_topic_property_; + + void drawBackground(QPainter & painter, const QRectF & backgroundRect); + void setupRosSubscriptions(); + + std::unique_ptr steering_wheel_display_; + std::unique_ptr gear_display_; + std::unique_ptr speed_display_; + std::unique_ptr turn_signals_display_; + std::unique_ptr traffic_display_; + std::unique_ptr speed_limit_display_; + + rclcpp::Subscription::SharedPtr gear_sub_; + rclcpp::Subscription::SharedPtr steering_sub_; + rclcpp::Subscription::SharedPtr speed_sub_; + rclcpp::Subscription::SharedPtr + turn_signals_sub_; + rclcpp::Subscription::SharedPtr + hazard_lights_sub_; + rclcpp::Subscription::SharedPtr traffic_sub_; + rclcpp::Subscription::SharedPtr speed_limit_sub_; + + std::mutex property_mutex_; + + void updateGearData(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); + void updateSteeringData( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); + void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + void updateTurnSignalsData( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); + void updateHazardLightsData( + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); + void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); + void updateTrafficLightData( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); + void drawWidget(QImage & hud); +}; +} // namespace awf_2d_overlay_vehicle + +#endif // SIGNAL_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_display.hpp new file mode 100644 index 0000000000000..317e45917f927 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_display.hpp @@ -0,0 +1,49 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SPEED_DISPLAY_HPP_ +#define SPEED_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" + +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +class SpeedDisplay +{ +public: + SpeedDisplay(); + void drawSpeedDisplay(QPainter & painter, const QRectF & backgroundRect); + void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + +private: + float current_speed_; // Internal variable to store current speed + QColor gray = QColor(194, 194, 194); +}; + +} // namespace awf_2d_overlay_vehicle + +#endif // SPEED_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_limit_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_limit_display.hpp new file mode 100644 index 0000000000000..00eae077ff2ac --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/speed_limit_display.hpp @@ -0,0 +1,49 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SPEED_LIMIT_DISPLAY_HPP_ +#define SPEED_LIMIT_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +class SpeedLimitDisplay +{ +public: + SpeedLimitDisplay(); + void drawSpeedLimitIndicator(QPainter & painter, const QRectF & backgroundRect); + void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); + +private: + float current_limit; // Internal variable to store current gear + QColor gray = QColor(194, 194, 194); +}; + +} // namespace awf_2d_overlay_vehicle + +#endif // SPEED_LIMIT_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/steering_wheel_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/steering_wheel_display.hpp new file mode 100644 index 0000000000000..a8291064c3807 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/steering_wheel_display.hpp @@ -0,0 +1,54 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef STEERING_WHEEL_DISPLAY_HPP_ +#define STEERING_WHEEL_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" + +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +class SteeringWheelDisplay +{ +public: + SteeringWheelDisplay(); + void drawSteeringWheel(QPainter & painter, const QRectF & backgroundRect); + void updateSteeringData( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); + +private: + float steering_angle_ = 0.0f; + QColor gray = QColor(194, 194, 194); + + QImage wheelImage; + QImage scaledWheelImage; + QImage coloredImage(const QImage & source, const QColor & color); +}; + +} // namespace awf_2d_overlay_vehicle + +#endif // STEERING_WHEEL_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/traffic_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/traffic_display.hpp new file mode 100644 index 0000000000000..bf776d27dfa94 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/traffic_display.hpp @@ -0,0 +1,62 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAFFIC_DISPLAY_HPP_ +#define TRAFFIC_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +class TrafficDisplay +{ +public: + TrafficDisplay(); + void drawTrafficLightIndicator(QPainter & painter, const QRectF & backgroundRect); + void updateTrafficLightData( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr & msg); + autoware_perception_msgs::msg::TrafficSignalArray current_traffic_; + +private: + QImage traffic_light_image_; + // yellow #CFC353 + QColor yellow = QColor(207, 195, 83); + // red #CF5353 + QColor red = QColor(207, 83, 83); + // green #53CF5F + QColor green = QColor(83, 207, 95); + // gray #C2C2C2 + QColor gray = QColor(194, 194, 194); + + QImage coloredImage(const QImage & source, const QColor & color); +}; + +} // namespace awf_2d_overlay_vehicle + +#endif // TRAFFIC_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/turn_signals_display.hpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/turn_signals_display.hpp new file mode 100644 index 0000000000000..cd659883c9ca0 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/include/turn_signals_display.hpp @@ -0,0 +1,63 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TURN_SIGNALS_DISPLAY_HPP_ +#define TURN_SIGNALS_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include + +namespace awf_2d_overlay_vehicle +{ + +class TurnSignalsDisplay +{ +public: + TurnSignalsDisplay(); + void drawArrows(QPainter & painter, const QRectF & backgroundRect, const QColor & color); + void updateTurnSignalsData( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); + void updateHazardLightsData( + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); + +private: + QImage arrowImage; + QColor gray = QColor(194, 194, 194); + + int current_turn_signal_; // Internal variable to store turn signal state + int current_hazard_lights_; // Internal variable to store hazard lights state + QImage coloredImage(const QImage & source, const QColor & color); + + std::chrono::steady_clock::time_point last_toggle_time_; + bool blink_on_ = false; + const std::chrono::milliseconds blink_interval_{500}; // Blink interval in milliseconds +}; + +} // namespace awf_2d_overlay_vehicle + +#endif // TURN_SIGNALS_DISPLAY_HPP_ diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/package.xml b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/package.xml new file mode 100644 index 0000000000000..b19b384d020b6 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/package.xml @@ -0,0 +1,31 @@ + + + + awf_2d_overlay_vehicle + 0.0.1 + + RViz2 plugin for 2D overlays in the 3D view. Mainly a port of the JSK overlay plugin + (https://github.com/jsk-ros-pkg/jsk_visualization). + + Khalil Selyan + + BSD-3-Clause + + autoware_auto_vehicle_msgs + autoware_perception_msgs + boost + rviz_2d_overlay_msgs + rviz_common + rviz_ogre_vendor + rviz_rendering + tier4_planning_msgs + + ament_lint_auto + autoware_lint_common + + ament_cmake + + + ament_cmake + + diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/plugins_description.xml b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/plugins_description.xml new file mode 100644 index 0000000000000..8a5af0abcf0dd --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/plugins_description.xml @@ -0,0 +1,5 @@ + + + Signal overlay plugin for the 3D view. + + diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/gear_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/gear_display.cpp new file mode 100644 index 0000000000000..153e106f04264 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/gear_display.cpp @@ -0,0 +1,98 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gear_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +GearDisplay::GearDisplay() : current_gear_(0) +{ + std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + std::string font_path = package_path + "/assets/font/Quicksand/static/Quicksand-Regular.ttf"; + std::string font_path2 = package_path + "/assets/font/Quicksand/static/Quicksand-Bold.ttf"; + int fontId = QFontDatabase::addApplicationFont( + font_path.c_str()); // returns -1 on failure (see docs for more info) + int fontId2 = QFontDatabase::addApplicationFont( + font_path2.c_str()); // returns -1 on failure (see docs for more info) + if (fontId == -1 || fontId2 == -1) { + std::cout << "Failed to load the Quicksand font."; + } +} + +void GearDisplay::updateGearData( + const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) +{ + current_gear_ = msg->report; // Assuming msg->report contains the gear information +} + +void GearDisplay::drawGearIndicator(QPainter & painter, const QRectF & backgroundRect) +{ + // we deal with the different gears here + std::string gearString; + switch (current_gear_) { + case autoware_auto_vehicle_msgs::msg::GearReport::NEUTRAL: + gearString = "N"; + break; + case autoware_auto_vehicle_msgs::msg::GearReport::LOW: + case autoware_auto_vehicle_msgs::msg::GearReport::LOW_2: + gearString = "L"; + break; + case autoware_auto_vehicle_msgs::msg::GearReport::NONE: + case autoware_auto_vehicle_msgs::msg::GearReport::PARK: + gearString = "P"; + break; + case autoware_auto_vehicle_msgs::msg::GearReport::REVERSE: + case autoware_auto_vehicle_msgs::msg::GearReport::REVERSE_2: + gearString = "R"; + break; + // all the drive gears from DRIVE to DRIVE_16 + default: + gearString = "D"; + break; + } + + QFont gearFont("Quicksand", 16, QFont::Bold); + painter.setFont(gearFont); + QPen borderPen(gray); + borderPen.setWidth(4); + painter.setPen(borderPen); + + int gearBoxSize = 30; + int gearX = backgroundRect.left() + 30 + gearBoxSize; + int gearY = backgroundRect.height() - gearBoxSize - 20; + QRect gearRect(gearX, gearY, gearBoxSize, gearBoxSize); + painter.setBrush(QColor(0, 0, 0, 0)); + painter.drawRoundedRect(gearRect, 5, 5); + painter.drawText(gearRect, Qt::AlignCenter, QString::fromStdString(gearString)); +} + +} // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_text_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_text_display.cpp new file mode 100644 index 0000000000000..053d0f6e981c0 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_text_display.cpp @@ -0,0 +1,556 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// -*- mode: c++; -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Team Spatzenhirn + * Copyright (c) 2014, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "overlay_text_display.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +namespace awf_2d_overlay_vehicle +{ +OverlayTextDisplay::OverlayTextDisplay() +: texture_width_(0), + texture_height_(0), + bg_color_(0, 0, 0, 0), + fg_color_(255, 255, 255, 255.0), + text_size_(14), + line_width_(2), + text_(""), + font_(""), + require_update_texture_(false) +{ + overtake_position_properties_property_ = new rviz_common::properties::BoolProperty( + "Overtake Position Properties", false, + "overtake position properties specified by message such as left, top and font", this, + SLOT(updateOvertakePositionProperties())); + overtake_fg_color_properties_property_ = new rviz_common::properties::BoolProperty( + "Overtake FG Color Properties", false, + "overtake color properties specified by message such as foreground color and alpha", this, + SLOT(updateOvertakeFGColorProperties())); + overtake_bg_color_properties_property_ = new rviz_common::properties::BoolProperty( + "Overtake BG Color Properties", false, + "overtake color properties specified by message such as background color and alpha", this, + SLOT(updateOvertakeBGColorProperties())); + align_bottom_property_ = new rviz_common::properties::BoolProperty( + "Align Bottom", false, "align text with the bottom of the overlay region", this, + SLOT(updateAlignBottom())); + invert_shadow_property_ = new rviz_common::properties::BoolProperty( + "Invert Shadow", false, "make shadow lighter than original text", this, + SLOT(updateInvertShadow())); + hor_dist_property_ = new rviz_common::properties::IntProperty( + "hor_dist", 0, "horizontal distance to anchor", this, SLOT(updateHorizontalDistance())); + ver_dist_property_ = new rviz_common::properties::IntProperty( + "ver_dist", 0, "vertical distance to anchor", this, SLOT(updateVerticalDistance())); + hor_alignment_property_ = new rviz_common::properties::EnumProperty( + "hor_alignment", "left", "horizontal alignment of the overlay", this, + SLOT(updateHorizontalAlignment())); + hor_alignment_property_->addOption("left", rviz_2d_overlay_msgs::msg::OverlayText::LEFT); + hor_alignment_property_->addOption("center", rviz_2d_overlay_msgs::msg::OverlayText::CENTER); + hor_alignment_property_->addOption("right", rviz_2d_overlay_msgs::msg::OverlayText::RIGHT); + ver_alignment_property_ = new rviz_common::properties::EnumProperty( + "ver_alignment", "top", "vertical alignment of the overlay", this, + SLOT(updateVerticalAlignment())); + ver_alignment_property_->addOption("top", rviz_2d_overlay_msgs::msg::OverlayText::TOP); + ver_alignment_property_->addOption("center", rviz_2d_overlay_msgs::msg::OverlayText::CENTER); + ver_alignment_property_->addOption("bottom", rviz_2d_overlay_msgs::msg::OverlayText::BOTTOM); + width_property_ = new rviz_common::properties::IntProperty( + "width", 128, "width position", this, SLOT(updateWidth())); + width_property_->setMin(0); + height_property_ = new rviz_common::properties::IntProperty( + "height", 128, "height position", this, SLOT(updateHeight())); + height_property_->setMin(0); + text_size_property_ = new rviz_common::properties::IntProperty( + "text size", 12, "text size", this, SLOT(updateTextSize())); + text_size_property_->setMin(0); + line_width_property_ = new rviz_common::properties::IntProperty( + "line width", 2, "line width", this, SLOT(updateLineWidth())); + line_width_property_->setMin(0); + fg_color_property_ = new rviz_common::properties::ColorProperty( + "Foreground Color", QColor(25, 255, 240), "Foreground Color", this, SLOT(updateFGColor())); + fg_alpha_property_ = new rviz_common::properties::FloatProperty( + "Foreground Alpha", 0.8, "Foreground Alpha", this, SLOT(updateFGAlpha())); + fg_alpha_property_->setMin(0.0); + fg_alpha_property_->setMax(1.0); + bg_color_property_ = new rviz_common::properties::ColorProperty( + "Background Color", QColor(0, 0, 0), "Background Color", this, SLOT(updateBGColor())); + bg_alpha_property_ = new rviz_common::properties::FloatProperty( + "Background Alpha", 0.8, "Background Alpha", this, SLOT(updateBGAlpha())); + bg_alpha_property_->setMin(0.0); + bg_alpha_property_->setMax(1.0); + + QFontDatabase database; + font_families_ = database.families(); + font_property_ = new rviz_common::properties::EnumProperty( + "font", "DejaVu Sans Mono", "font", this, SLOT(updateFont())); + for (ssize_t i = 0; i < font_families_.size(); i++) { + font_property_->addOption(font_families_[i], static_cast(i)); + } +} + +OverlayTextDisplay::~OverlayTextDisplay() +{ + onDisable(); +} + +void OverlayTextDisplay::onEnable() +{ + if (overlay_) { + overlay_->show(); + } + subscribe(); +} + +void OverlayTextDisplay::onDisable() +{ + if (overlay_) { + overlay_->hide(); + } + unsubscribe(); +} + +// only the first time +void OverlayTextDisplay::onInitialize() +{ + RTDClass::onInitialize(); + rviz_rendering::RenderSystem::get()->prepareOverlays(scene_manager_); + + onEnable(); + updateTopic(); + updateOvertakePositionProperties(); + updateOvertakeFGColorProperties(); + updateOvertakeBGColorProperties(); + updateAlignBottom(); + updateInvertShadow(); + updateHorizontalDistance(); + updateVerticalDistance(); + updateHorizontalAlignment(); + updateVerticalAlignment(); + updateWidth(); + updateHeight(); + updateTextSize(); + updateFGColor(); + updateFGAlpha(); + updateBGColor(); + updateBGAlpha(); + updateFont(); + updateLineWidth(); + require_update_texture_ = true; +} + +void OverlayTextDisplay::update(float /*wall_dt*/, float /*ros_dt*/) +{ + if (!require_update_texture_) { + return; + } + if (!isEnabled()) { + return; + } + if (!overlay_) { + return; + } + + overlay_->updateTextureSize(texture_width_, texture_height_); + { + awf_2d_overlay_vehicle::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage Hud = buffer.getQImage(*overlay_, bg_color_); + QPainter painter(&Hud); + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setPen(QPen(fg_color_, std::max(line_width_, 1), Qt::SolidLine)); + uint16_t w = overlay_->getTextureWidth(); + uint16_t h = overlay_->getTextureHeight(); + + // font + if (text_size_ != 0) { + // QFont font = painter.font(); + QFont font(font_.length() > 0 ? font_.c_str() : "Liberation Sans"); + font.setPointSize(text_size_); + font.setBold(true); + painter.setFont(font); + } + if (text_.length() > 0) { + QColor shadow_color; + if (invert_shadow_) + shadow_color = Qt::white; // fg_color_.lighter(); + else + shadow_color = Qt::black; // fg_color_.darker(); + shadow_color.setAlpha(fg_color_.alpha()); + + std::string color_wrapped_text = + (boost::format("%1%") % text_ % + fg_color_.red() % fg_color_.green() % fg_color_.blue() % fg_color_.alpha()) + .str(); + + // find a remove "color: XXX;" regex match to generate a proper shadow + std::regex color_tag_re("color:.+?;"); + std::string null_char(""); + std::string formatted_text_ = std::regex_replace(text_, color_tag_re, null_char); + std::string color_wrapped_shadow = + (boost::format("%1%") % + formatted_text_ % shadow_color.red() % shadow_color.green() % shadow_color.blue() % + shadow_color.alpha()) + .str(); + + QStaticText static_text( + boost::algorithm::replace_all_copy(color_wrapped_text, "\n", "
").c_str()); + static_text.setTextWidth(w); + + painter.setPen(QPen(shadow_color, std::max(line_width_, 1), Qt::SolidLine)); + QStaticText static_shadow( + boost::algorithm::replace_all_copy(color_wrapped_shadow, "\n", "
").c_str()); + static_shadow.setTextWidth(w); + + if (!align_bottom_) { + painter.drawStaticText(1, 1, static_shadow); + painter.drawStaticText(0, 0, static_text); + } else { + QStaticText only_wrapped_text(color_wrapped_text.c_str()); + QFontMetrics fm(painter.fontMetrics()); + QRect text_rect = fm.boundingRect( + 0, 0, w, h, Qt::TextWordWrap | Qt::AlignLeft | Qt::AlignTop, + only_wrapped_text.text().remove(QRegExp("<[^>]*>"))); + painter.drawStaticText(1, h - text_rect.height() + 1, static_shadow); + painter.drawStaticText(0, h - text_rect.height(), static_text); + } + } + painter.end(); + } + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); + require_update_texture_ = false; +} + +void OverlayTextDisplay::reset() +{ + RTDClass::reset(); + + if (overlay_) { + overlay_->hide(); + } +} + +void OverlayTextDisplay::processMessage(rviz_2d_overlay_msgs::msg::OverlayText::ConstSharedPtr msg) +{ + if (!isEnabled()) { + return; + } + if (!overlay_) { + static int count = 0; + std::stringstream ss; + ss << "OverlayTextDisplayObject" << count++; + overlay_.reset(new awf_2d_overlay_vehicle::OverlayObject(ss.str())); + overlay_->show(); + } + if (overlay_) { + if (msg->action == rviz_2d_overlay_msgs::msg::OverlayText::DELETE) { + overlay_->hide(); + } else if (msg->action == rviz_2d_overlay_msgs::msg::OverlayText::ADD) { + overlay_->show(); + } + } + + // store message for update method + text_ = msg->text; + + if (!overtake_position_properties_) { + texture_width_ = msg->width; + texture_height_ = msg->height; + text_size_ = msg->text_size; + horizontal_dist_ = msg->horizontal_distance; + vertical_dist_ = msg->vertical_distance; + + horizontal_alignment_ = HorizontalAlignment{msg->horizontal_alignment}; + vertical_alignment_ = VerticalAlignment{msg->vertical_alignment}; + } + if (!overtake_bg_color_properties_) + bg_color_ = QColor( + msg->bg_color.r * 255.0, msg->bg_color.g * 255.0, msg->bg_color.b * 255.0, + msg->bg_color.a * 255.0); + if (!overtake_fg_color_properties_) { + fg_color_ = QColor( + msg->fg_color.r * 255.0, msg->fg_color.g * 255.0, msg->fg_color.b * 255.0, + msg->fg_color.a * 255.0); + font_ = msg->font; + line_width_ = msg->line_width; + } + if (overlay_) { + overlay_->setPosition( + horizontal_dist_, vertical_dist_, horizontal_alignment_, vertical_alignment_); + } + require_update_texture_ = true; +} + +void OverlayTextDisplay::updateOvertakePositionProperties() +{ + if (!overtake_position_properties_ && overtake_position_properties_property_->getBool()) { + updateVerticalDistance(); + updateHorizontalDistance(); + updateVerticalAlignment(); + updateHorizontalAlignment(); + updateWidth(); + updateHeight(); + updateTextSize(); + require_update_texture_ = true; + } + + overtake_position_properties_ = overtake_position_properties_property_->getBool(); + if (overtake_position_properties_) { + hor_dist_property_->show(); + ver_dist_property_->show(); + hor_alignment_property_->show(); + ver_alignment_property_->show(); + width_property_->show(); + height_property_->show(); + text_size_property_->show(); + } else { + hor_dist_property_->hide(); + ver_dist_property_->hide(); + hor_alignment_property_->hide(); + ver_alignment_property_->hide(); + width_property_->hide(); + height_property_->hide(); + text_size_property_->hide(); + } +} + +void OverlayTextDisplay::updateOvertakeFGColorProperties() +{ + if (!overtake_fg_color_properties_ && overtake_fg_color_properties_property_->getBool()) { + // read all the parameters from properties + updateFGColor(); + updateFGAlpha(); + updateFont(); + updateLineWidth(); + require_update_texture_ = true; + } + overtake_fg_color_properties_ = overtake_fg_color_properties_property_->getBool(); + if (overtake_fg_color_properties_) { + fg_color_property_->show(); + fg_alpha_property_->show(); + line_width_property_->show(); + font_property_->show(); + } else { + fg_color_property_->hide(); + fg_alpha_property_->hide(); + line_width_property_->hide(); + font_property_->hide(); + } +} + +void OverlayTextDisplay::updateOvertakeBGColorProperties() +{ + if (!overtake_bg_color_properties_ && overtake_bg_color_properties_property_->getBool()) { + // read all the parameters from properties + updateBGColor(); + updateBGAlpha(); + require_update_texture_ = true; + } + overtake_bg_color_properties_ = overtake_bg_color_properties_property_->getBool(); + if (overtake_bg_color_properties_) { + bg_color_property_->show(); + bg_alpha_property_->show(); + } else { + bg_color_property_->hide(); + bg_alpha_property_->hide(); + } +} + +void OverlayTextDisplay::updateAlignBottom() +{ + if (align_bottom_ != align_bottom_property_->getBool()) { + require_update_texture_ = true; + } + align_bottom_ = align_bottom_property_->getBool(); +} + +void OverlayTextDisplay::updateInvertShadow() +{ + if (invert_shadow_ != invert_shadow_property_->getBool()) { + require_update_texture_ = true; + } + invert_shadow_ = invert_shadow_property_->getBool(); +} + +void OverlayTextDisplay::updateVerticalDistance() +{ + vertical_dist_ = ver_dist_property_->getInt(); + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateHorizontalDistance() +{ + horizontal_dist_ = hor_dist_property_->getInt(); + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateVerticalAlignment() +{ + vertical_alignment_ = + VerticalAlignment{static_cast(ver_alignment_property_->getOptionInt())}; + + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateHorizontalAlignment() +{ + horizontal_alignment_ = + HorizontalAlignment{static_cast(hor_alignment_property_->getOptionInt())}; + + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateWidth() +{ + texture_width_ = width_property_->getInt(); + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateHeight() +{ + texture_height_ = height_property_->getInt(); + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateTextSize() +{ + text_size_ = text_size_property_->getInt(); + if (overtake_position_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateBGColor() +{ + QColor c = bg_color_property_->getColor(); + bg_color_.setRed(c.red()); + bg_color_.setGreen(c.green()); + bg_color_.setBlue(c.blue()); + if (overtake_bg_color_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateBGAlpha() +{ + bg_color_.setAlpha(bg_alpha_property_->getFloat() * 255.0); + if (overtake_bg_color_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateFGColor() +{ + QColor c = fg_color_property_->getColor(); + fg_color_.setRed(c.red()); + fg_color_.setGreen(c.green()); + fg_color_.setBlue(c.blue()); + if (overtake_fg_color_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateFGAlpha() +{ + fg_color_.setAlpha(fg_alpha_property_->getFloat() * 255.0); + if (overtake_fg_color_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateFont() +{ + int font_index = font_property_->getOptionInt(); + if (font_index < font_families_.size()) { + font_ = font_families_[font_index].toStdString(); + } else { + RVIZ_COMMON_LOG_ERROR_STREAM("Unexpected error at selecting font index " << font_index); + return; + } + if (overtake_fg_color_properties_) { + require_update_texture_ = true; + } +} + +void OverlayTextDisplay::updateLineWidth() +{ + line_width_ = line_width_property_->getInt(); + if (overtake_fg_color_properties_) { + require_update_texture_ = true; + } +} + +} // namespace awf_2d_overlay_vehicle + +#include +PLUGINLIB_EXPORT_CLASS(awf_2d_overlay_vehicle::OverlayTextDisplay, rviz_common::Display) diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_utils.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_utils.cpp new file mode 100644 index 0000000000000..e65a74415fb6a --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/overlay_utils.cpp @@ -0,0 +1,267 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Team Spatzenhirn + * Copyright (c) 2014, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "overlay_utils.hpp" + +#include + +namespace awf_2d_overlay_vehicle +{ +ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer) +: pixel_buffer_(pixel_buffer) +{ + pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL); +} + +ScopedPixelBuffer::~ScopedPixelBuffer() +{ + pixel_buffer_->unlock(); +} + +Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() +{ + return pixel_buffer_; +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height) +{ + const Ogre::PixelBox & pixelBox = pixel_buffer_->getCurrentLock(); + Ogre::uint8 * pDest = static_cast(pixelBox.data); + memset(pDest, 0, width * height); + return QImage(pDest, width, height, QImage::Format_ARGB32); +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height, QColor & bg_color) +{ + QImage Hud = getQImage(width, height); + for (unsigned int i = 0; i < width; i++) { + for (unsigned int j = 0; j < height; j++) { + Hud.setPixel(i, j, bg_color.rgba()); + } + } + return Hud; +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight()); +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay, QColor & bg_color) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight(), bg_color); +} + +OverlayObject::OverlayObject(const std::string & name) : name_(name) +{ + std::string material_name = name_ + "Material"; + Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + overlay_ = mOverlayMgr->create(name_); + panel_ = static_cast( + mOverlayMgr->createOverlayElement("Panel", name_ + "Panel")); + panel_->setMetricsMode(Ogre::GMM_PIXELS); + + panel_material_ = Ogre::MaterialManager::getSingleton().create( + material_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + panel_->setMaterialName(panel_material_->getName()); + overlay_->add2D(panel_); +} + +OverlayObject::~OverlayObject() +{ + Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + if (mOverlayMgr) { + mOverlayMgr->destroyOverlayElement(panel_); + mOverlayMgr->destroy(overlay_); + } + if (panel_material_) { + panel_material_->unload(); + Ogre::MaterialManager::getSingleton().remove(panel_material_->getName()); + } +} + +std::string OverlayObject::getName() const +{ + return name_; +} + +void OverlayObject::hide() +{ + if (overlay_->isVisible()) { + overlay_->hide(); + } +} + +void OverlayObject::show() +{ + if (!overlay_->isVisible()) { + overlay_->show(); + } +} + +bool OverlayObject::isTextureReady() const +{ + return texture_ != nullptr; +} + +void OverlayObject::updateTextureSize(unsigned int width, unsigned int height) +{ + const std::string texture_name = name_ + "Texture"; + if (width == 0) { + RVIZ_COMMON_LOG_WARNING_STREAM("[OverlayObject] width=0 is specified as texture size"); + width = 1; + } + + if (height == 0) { + RVIZ_COMMON_LOG_WARNING_STREAM("[OverlayObject] height=0 is specified as texture size"); + height = 1; + } + + if (!isTextureReady() || ((width != texture_->getWidth()) || (height != texture_->getHeight()))) { + if (isTextureReady()) { + Ogre::TextureManager::getSingleton().remove(texture_name); + panel_material_->getTechnique(0)->getPass(0)->removeAllTextureUnitStates(); + } + + texture_ = Ogre::TextureManager::getSingleton().createManual( + texture_name, // name + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, // type + width, height, // width & height of the render window + 0, // number of mipmaps + Ogre::PF_A8R8G8B8, // pixel format chosen to match a format Qt can use + Ogre::TU_DEFAULT // usage + ); + panel_material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_name); + + panel_material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + } +} + +ScopedPixelBuffer OverlayObject::getBuffer() +{ + if (isTextureReady()) { + return ScopedPixelBuffer(texture_->getBuffer()); + } else { + return ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr()); + } +} + +void OverlayObject::setPosition( + double hor_dist, double ver_dist, HorizontalAlignment hor_alignment, + VerticalAlignment ver_alignment) +{ + // ogre position is always based on the top left corner of the panel, while our position input + // depends on the chosen alignment, i.e. if the horizontal alignment is right, increasing the + // horizontal dist moves the panel to the left (further away from the right border) + double left = 0; + double top = 0; + + switch (hor_alignment) { + case HorizontalAlignment::LEFT: + panel_->setHorizontalAlignment(Ogre::GuiHorizontalAlignment::GHA_LEFT); + left = hor_dist; + break; + case HorizontalAlignment::CENTER: + panel_->setHorizontalAlignment(Ogre::GuiHorizontalAlignment::GHA_CENTER); + left = hor_dist - panel_->getWidth() / 2; + break; + case HorizontalAlignment::RIGHT: + panel_->setHorizontalAlignment(Ogre::GuiHorizontalAlignment::GHA_RIGHT); + left = -hor_dist - panel_->getWidth(); + break; + } + + switch (ver_alignment) { + case VerticalAlignment::BOTTOM: + panel_->setVerticalAlignment(Ogre::GuiVerticalAlignment::GVA_BOTTOM); + top = -ver_dist - panel_->getHeight(); + break; + case VerticalAlignment::CENTER: + panel_->setVerticalAlignment(Ogre::GuiVerticalAlignment::GVA_CENTER); + top = ver_dist - panel_->getHeight() / 2; + break; + case VerticalAlignment::TOP: + panel_->setVerticalAlignment(Ogre::GuiVerticalAlignment::GVA_TOP); + top = ver_dist; + break; + } + + panel_->setPosition(left, top); +} + +void OverlayObject::setDimensions(double width, double height) +{ + panel_->setDimensions(width, height); +} + +bool OverlayObject::isVisible() const +{ + return overlay_->isVisible(); +} + +unsigned int OverlayObject::getTextureWidth() const +{ + if (isTextureReady()) { + return texture_->getWidth(); + } else { + return 0; + } +} + +unsigned int OverlayObject::getTextureHeight() const +{ + if (isTextureReady()) { + return texture_->getHeight(); + } else { + return 0; + } +} +} // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/signal_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/signal_display.cpp new file mode 100644 index 0000000000000..f2a82dd386b37 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/signal_display.cpp @@ -0,0 +1,501 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "signal_display.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +SignalDisplay::SignalDisplay() +{ + property_width_ = new rviz_common::properties::IntProperty( + "Width", 517, "Width of the overlay", this, SLOT(updateOverlaySize())); + property_height_ = new rviz_common::properties::IntProperty( + "Height", 175, "Height of the overlay", this, SLOT(updateOverlaySize())); + property_left_ = new rviz_common::properties::IntProperty( + "Left", 10, "Left position of the overlay", this, SLOT(updateOverlayPosition())); + property_top_ = new rviz_common::properties::IntProperty( + "Top", 10, "Top position of the overlay", this, SLOT(updateOverlayPosition())); + property_signal_color_ = new rviz_common::properties::ColorProperty( + "Signal Color", QColor(94, 130, 255), "Color of the signal arrows", this, + SLOT(updateOverlayColor())); + + // Initialize the component displays + steering_wheel_display_ = std::make_unique(); + gear_display_ = std::make_unique(); + speed_display_ = std::make_unique(); + turn_signals_display_ = std::make_unique(); + traffic_display_ = std::make_unique(); + speed_limit_display_ = std::make_unique(); +} + +void SignalDisplay::onInitialize() +{ + std::lock_guard lock(property_mutex_); + + rviz_common::Display::onInitialize(); + rviz_rendering::RenderSystem::get()->prepareOverlays(scene_manager_); + static int count = 0; + std::stringstream ss; + ss << "SignalDisplayObject" << count++; + overlay_.reset(new awf_2d_overlay_vehicle::OverlayObject(ss.str())); + overlay_->show(); + updateOverlaySize(); + updateOverlayPosition(); + + auto rviz_ros_node = context_->getRosNodeAbstraction(); + + gear_topic_property_ = std::make_unique( + "Gear Topic Test", "/vehicle/status/gear_status", "autoware_auto_vehicle_msgs/msg/GearReport", + "Topic for Gear Data", this, SLOT(topic_updated_gear())); + gear_topic_property_->initialize(rviz_ros_node); + + turn_signals_topic_property_ = std::make_unique( + "Turn Signals Topic", "/vehicle/status/turn_indicators_status", + "autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport", "Topic for Turn Signals Data", this, + SLOT(topic_updated_turn_signals())); + turn_signals_topic_property_->initialize(rviz_ros_node); + + speed_topic_property_ = std::make_unique( + "Speed Topic", "/vehicle/status/velocity_status", + "autoware_auto_vehicle_msgs/msg/VelocityReport", "Topic for Speed Data", this, + SLOT(topic_updated_speed())); + speed_topic_property_->initialize(rviz_ros_node); + + steering_topic_property_ = std::make_unique( + "Steering Topic", "/vehicle/status/steering_status", + "autoware_auto_vehicle_msgs/msg/SteeringReport", "Topic for Steering Data", this, + SLOT(topic_updated_steering())); + steering_topic_property_->initialize(rviz_ros_node); + + hazard_lights_topic_property_ = std::make_unique( + "Hazard Lights Topic", "/vehicle/status/hazard_lights_status", + "autoware_auto_vehicle_msgs/msg/HazardLightsReport", "Topic for Hazard Lights Data", this, + SLOT(topic_updated_hazard_lights())); + hazard_lights_topic_property_->initialize(rviz_ros_node); + + speed_limit_topic_property_ = std::make_unique( + "Speed Limit Topic", "/planning/scenario_planning/current_max_velocity", + "tier4_planning_msgs/msg/VelocityLimit", "Topic for Speed Limit Data", this, + SLOT(topic_updated_speed_limit())); + speed_limit_topic_property_->initialize(rviz_ros_node); + + traffic_topic_property_ = std::make_unique( + "Traffic Topic", "/perception/traffic_light_recognition/traffic_signals", + "autoware_perception/msgs/msg/TrafficSignalArray", "Topic for Traffic Light Data", this, + SLOT(topic_updated_traffic())); + traffic_topic_property_->initialize(rviz_ros_node); +} + +void SignalDisplay::setupRosSubscriptions() +{ + // Don't create a node, just use the one from the parent class + auto rviz_node_ = context_->getRosNodeAbstraction().lock()->get_raw_node(); + + gear_sub_ = rviz_node_->create_subscription( + gear_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg) { + updateGearData(msg); + }); + + steering_sub_ = rviz_node_->create_subscription( + steering_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) { + updateSteeringData(msg); + }); + + speed_sub_ = rviz_node_->create_subscription( + speed_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg) { + updateSpeedData(msg); + }); + + turn_signals_sub_ = + rviz_node_->create_subscription( + turn_signals_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::SharedPtr msg) { + updateTurnSignalsData(msg); + }); + + hazard_lights_sub_ = + rviz_node_->create_subscription( + hazard_lights_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::HazardLightsReport::SharedPtr msg) { + updateHazardLightsData(msg); + }); + + traffic_sub_ = rviz_node_->create_subscription( + traffic_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_perception_msgs::msg::TrafficSignalArray::SharedPtr msg) { + updateTrafficLightData(msg); + }); + + speed_limit_sub_ = rviz_node_->create_subscription( + speed_limit_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) { + updateSpeedLimitData(msg); + }); +} + +SignalDisplay::~SignalDisplay() +{ + std::lock_guard lock(property_mutex_); + overlay_.reset(); + + gear_sub_.reset(); + steering_sub_.reset(); + speed_sub_.reset(); + turn_signals_sub_.reset(); + hazard_lights_sub_.reset(); + traffic_sub_.reset(); + + steering_wheel_display_.reset(); + gear_display_.reset(); + speed_display_.reset(); + turn_signals_display_.reset(); + traffic_display_.reset(); + speed_limit_display_.reset(); + + gear_topic_property_.reset(); + turn_signals_topic_property_.reset(); + speed_topic_property_.reset(); + steering_topic_property_.reset(); + hazard_lights_topic_property_.reset(); + traffic_topic_property_.reset(); +} + +void SignalDisplay::update(float /* wall_dt */, float /* ros_dt */) +{ + if (!overlay_) { + return; + } + awf_2d_overlay_vehicle::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(Qt::transparent); + drawWidget(hud); +} + +void SignalDisplay::onEnable() +{ + std::lock_guard lock(property_mutex_); + if (overlay_) { + overlay_->show(); + } + setupRosSubscriptions(); +} + +void SignalDisplay::onDisable() +{ + std::lock_guard lock(property_mutex_); + + gear_sub_.reset(); + steering_sub_.reset(); + speed_sub_.reset(); + turn_signals_sub_.reset(); + hazard_lights_sub_.reset(); + + if (overlay_) { + overlay_->hide(); + } +} + +void SignalDisplay::updateTrafficLightData( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) +{ + std::lock_guard lock(property_mutex_); + + if (traffic_display_) { + traffic_display_->updateTrafficLightData(msg); + } +} + +void SignalDisplay::updateSpeedLimitData( + const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) +{ + std::lock_guard lock(property_mutex_); + + if (speed_limit_display_) { + speed_limit_display_->updateSpeedLimitData(msg); + queueRender(); + } +} + +void SignalDisplay::updateHazardLightsData( + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (turn_signals_display_) { + turn_signals_display_->updateHazardLightsData(msg); + queueRender(); + } +} + +void SignalDisplay::updateGearData( + const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (gear_display_) { + gear_display_->updateGearData(msg); + queueRender(); + } +} + +void SignalDisplay::updateSteeringData( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (steering_wheel_display_) { + steering_wheel_display_->updateSteeringData(msg); + queueRender(); + } +} + +void SignalDisplay::updateSpeedData( + const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (speed_display_) { + speed_display_->updateSpeedData(msg); + queueRender(); + } +} + +void SignalDisplay::updateTurnSignalsData( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (turn_signals_display_) { + turn_signals_display_->updateTurnSignalsData(msg); + queueRender(); + } +} + +void SignalDisplay::drawWidget(QImage & hud) +{ + std::lock_guard lock(property_mutex_); + + if (!overlay_->isVisible()) { + return; + } + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + QRectF backgroundRect(0, 0, 322, hud.height()); + drawBackground(painter, backgroundRect); + + // Draw components + if (steering_wheel_display_) { + steering_wheel_display_->drawSteeringWheel(painter, backgroundRect); + } + if (gear_display_) { + gear_display_->drawGearIndicator(painter, backgroundRect); + } + if (speed_display_) { + speed_display_->drawSpeedDisplay(painter, backgroundRect); + } + if (turn_signals_display_) { + turn_signals_display_->drawArrows(painter, backgroundRect, property_signal_color_->getColor()); + } + + // a 27px space between the two halves of the HUD + + QRectF smallerBackgroundRect(349, 0, 168, hud.height() / 2); + + drawBackground(painter, smallerBackgroundRect); + + if (traffic_display_) { + traffic_display_->drawTrafficLightIndicator(painter, smallerBackgroundRect); + } + + if (speed_limit_display_) { + speed_limit_display_->drawSpeedLimitIndicator(painter, smallerBackgroundRect); + } + + painter.end(); +} + +void SignalDisplay::drawBackground(QPainter & painter, const QRectF & backgroundRect) +{ + painter.setBrush(QColor(0, 0, 0, 255 * 0.2)); // Black background with opacity + painter.setPen(Qt::NoPen); + painter.drawRoundedRect( + backgroundRect, backgroundRect.height() / 2, backgroundRect.height() / 2); // Circular ends +} + +void SignalDisplay::reset() +{ + rviz_common::Display::reset(); + overlay_->hide(); +} + +void SignalDisplay::updateOverlaySize() +{ + std::lock_guard lock(mutex_); + overlay_->updateTextureSize(property_width_->getInt(), property_height_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); + queueRender(); +} + +void SignalDisplay::updateOverlayPosition() +{ + std::lock_guard lock(mutex_); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + queueRender(); +} + +void SignalDisplay::updateOverlayColor() +{ + std::lock_guard lock(mutex_); + queueRender(); +} + +void SignalDisplay::topic_updated_gear() +{ + // resubscribe to the topic + gear_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + gear_sub_ = + rviz_ros_node->get_raw_node()->create_subscription( + gear_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg) { + updateGearData(msg); + }); +} + +void SignalDisplay::topic_updated_steering() +{ + // resubscribe to the topic + steering_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + steering_sub_ = rviz_ros_node->get_raw_node() + ->create_subscription( + steering_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) { + updateSteeringData(msg); + }); +} + +void SignalDisplay::topic_updated_speed() +{ + // resubscribe to the topic + speed_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + speed_sub_ = rviz_ros_node->get_raw_node() + ->create_subscription( + speed_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg) { + updateSpeedData(msg); + }); +} + +void SignalDisplay::topic_updated_speed_limit() +{ + // resubscribe to the topic + speed_limit_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + speed_limit_sub_ = + rviz_ros_node->get_raw_node()->create_subscription( + speed_limit_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) { + updateSpeedLimitData(msg); + }); +} + +void SignalDisplay::topic_updated_turn_signals() +{ + // resubscribe to the topic + turn_signals_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + + turn_signals_sub_ = + rviz_ros_node->get_raw_node() + ->create_subscription( + turn_signals_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::SharedPtr msg) { + updateTurnSignalsData(msg); + }); +} + +void SignalDisplay::topic_updated_hazard_lights() +{ + // resubscribe to the topic + hazard_lights_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + + hazard_lights_sub_ = + rviz_ros_node->get_raw_node() + ->create_subscription( + hazard_lights_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_auto_vehicle_msgs::msg::HazardLightsReport::SharedPtr msg) { + updateHazardLightsData(msg); + }); +} + +void SignalDisplay::topic_updated_traffic() +{ + // resubscribe to the topic + traffic_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + traffic_sub_ = + rviz_ros_node->get_raw_node() + ->create_subscription( + traffic_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_perception_msgs::msg::TrafficSignalArray::SharedPtr msg) { + updateTrafficLightData(msg); + }); +} + +} // namespace awf_2d_overlay_vehicle + +#include +PLUGINLIB_EXPORT_CLASS(awf_2d_overlay_vehicle::SignalDisplay, rviz_common::Display) diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_display.cpp new file mode 100644 index 0000000000000..8212758c09a9f --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_display.cpp @@ -0,0 +1,109 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "speed_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +SpeedDisplay::SpeedDisplay() : current_speed_(0.0) +{ + std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + std::string font_path = package_path + "/assets/font/Quicksand/static/Quicksand-Regular.ttf"; + std::string font_path2 = package_path + "/assets/font/Quicksand/static/Quicksand-Bold.ttf"; + int fontId = QFontDatabase::addApplicationFont( + font_path.c_str()); // returns -1 on failure (see docs for more info) + int fontId2 = QFontDatabase::addApplicationFont( + font_path2.c_str()); // returns -1 on failure (see docs for more info) + if (fontId == -1 || fontId2 == -1) { + std::cout << "Failed to load the Quicksand font."; + } +} + +void SpeedDisplay::updateSpeedData( + const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) +{ + try { + // Assuming msg->state.longitudinal_velocity_mps is the field you're interested in + float speed = msg->longitudinal_velocity; + // we received it as a m/s value, but we want to display it in km/h + current_speed_ = (speed * 3.6); + } catch (const std::exception & e) { + // Log the error + std::cerr << "Error in processMessage: " << e.what() << std::endl; + } +} + +// void SpeedDisplay::processMessage(const +// autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) +// { +// try +// { +// current_speed_ = std::round(msg->state.longitudinal_velocity_mps * 3.6); +// } +// catch (const std::exception &e) +// { +// std::cerr << "Error in processMessage: " << e.what() << std::endl; +// } +// } + +void SpeedDisplay::drawSpeedDisplay(QPainter & painter, const QRectF & backgroundRect) +{ + QFont referenceFont("Quicksand", 80, QFont::Bold); + painter.setFont(referenceFont); + QRect referenceRect = painter.fontMetrics().boundingRect("88"); + QPointF referencePos( + backgroundRect.width() / 2 - referenceRect.width() / 2 - 5, backgroundRect.height() / 2); + + QString speedNumber = QString::number(current_speed_, 'f', 0); + int fontSize = 60; + QFont speedFont("Quicksand", fontSize); + painter.setFont(speedFont); + + // Calculate the bounding box of the speed number + QRect speedNumberRect = painter.fontMetrics().boundingRect(speedNumber); + + // Center the speed number in the backgroundRect + QPointF speedPos( + backgroundRect.center().x() - speedNumberRect.width() / 2, backgroundRect.center().y()); + painter.setPen(gray); + painter.drawText(speedPos, speedNumber); + + QFont unitFont("Quicksand", 14); + painter.setFont(unitFont); + QString speedUnit = "km/h"; + QRect unitRect = painter.fontMetrics().boundingRect(speedUnit); + QPointF unitPos( + (backgroundRect.width() / 2 - unitRect.width() / 2), referencePos.y() + unitRect.height()); + painter.drawText(unitPos, speedUnit); +} + +} // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_limit_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_limit_display.cpp new file mode 100644 index 0000000000000..fcc1df998798b --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/speed_limit_display.cpp @@ -0,0 +1,105 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "speed_limit_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +SpeedLimitDisplay::SpeedLimitDisplay() : current_limit(0.0) +{ + std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + std::string font_path = package_path + "/assets/font/Quicksand/static/Quicksand-Regular.ttf"; + std::string font_path2 = package_path + "/assets/font/Quicksand/static/Quicksand-Bold.ttf"; + int fontId = QFontDatabase::addApplicationFont( + font_path.c_str()); // returns -1 on failure (see docs for more info) + int fontId2 = QFontDatabase::addApplicationFont( + font_path2.c_str()); // returns -1 on failure (see docs for more info) + if (fontId == -1 || fontId2 == -1) { + std::cout << "Failed to load the Quicksand font."; + } +} + +void SpeedLimitDisplay::updateSpeedLimitData( + const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) +{ + current_limit = msg->max_velocity; +} + +void SpeedLimitDisplay::drawSpeedLimitIndicator(QPainter & painter, const QRectF & backgroundRect) +{ + // Enable Antialiasing for smoother drawing + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setRenderHint(QPainter::SmoothPixmapTransform, true); + + // #C2C2C2 + painter.setPen(QPen(gray, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + painter.setBrush(QBrush(gray, Qt::SolidPattern)); + + // Define the area for the outer circle + QRectF outerCircleRect = backgroundRect; + outerCircleRect.setWidth(backgroundRect.width() / 2 - 20); + outerCircleRect.setHeight(backgroundRect.height() - 20); + outerCircleRect.moveTopLeft(QPointF(backgroundRect.left() + 10, backgroundRect.top() + 10)); + + // Define the area for the inner circle + QRectF innerCircleRect = outerCircleRect; + innerCircleRect.setWidth(outerCircleRect.width() / 1.375); + innerCircleRect.setHeight(outerCircleRect.height() / 1.375); + innerCircleRect.moveCenter(outerCircleRect.center()); + + // Draw the outer circle + painter.drawEllipse(outerCircleRect); + + // Change the composition mode and draw the inner circle + painter.setCompositionMode(QPainter::CompositionMode_Clear); + painter.drawEllipse(innerCircleRect); + + // Reset the composition mode + painter.setCompositionMode(QPainter::CompositionMode_SourceOver); + + int current_limit_int = std::round(current_limit * 3.6); + + // Define the text to be drawn + QString text = QString::number(current_limit_int); + + // Set the font and color for the text + QFont font = QFont("Quicksand", 14, QFont::Bold); + + painter.setFont(font); + // #C2C2C2 + painter.setPen(QPen(gray, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + + // Draw the text in the center of the circle + painter.drawText(innerCircleRect, Qt::AlignCenter, text); +} + +} // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/steering_wheel_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/steering_wheel_display.cpp new file mode 100644 index 0000000000000..b4da8ff5f8ffb --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/steering_wheel_display.cpp @@ -0,0 +1,123 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "steering_wheel_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +SteeringWheelDisplay::SteeringWheelDisplay() +{ + // Load the Quicksand font + std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + std::string font_path = package_path + "/assets/font/Quicksand/static/Quicksand-Regular.ttf"; + std::string font_path2 = package_path + "/assets/font/Quicksand/static/Quicksand-Bold.ttf"; + int fontId = QFontDatabase::addApplicationFont( + font_path.c_str()); // returns -1 on failure (see docs for more info) + int fontId2 = QFontDatabase::addApplicationFont( + font_path2.c_str()); // returns -1 on failure (see docs for more info) + if (fontId == -1 || fontId2 == -1) { + std::cout << "Failed to load the Quicksand font."; + } + + // Load the wheel image + std::string image_path = package_path + "/assets/images/wheel.png"; + wheelImage.load(image_path.c_str()); + scaledWheelImage = wheelImage.scaled(54, 54, Qt::KeepAspectRatio, Qt::SmoothTransformation); +} + +void SteeringWheelDisplay::updateSteeringData( + const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) +{ + try { + // Assuming msg->steering_angle is the field you're interested in + float steeringAngle = msg->steering_tire_angle; + // we received it as a radian value, but we want to display it in degrees + steering_angle_ = + (steeringAngle * -180 / M_PI) * + 17; // 17 is the ratio between the steering wheel and the steering tire angle i assume + } catch (const std::exception & e) { + // Log the error + std::cerr << "Error in processMessage: " << e.what() << std::endl; + } +} + +void SteeringWheelDisplay::drawSteeringWheel(QPainter & painter, const QRectF & backgroundRect) +{ + // Enable Antialiasing for smoother drawing + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setRenderHint(QPainter::SmoothPixmapTransform, true); + + QImage wheel = coloredImage(scaledWheelImage, gray); + + // Rotate the wheel + float steeringAngle = steering_angle_; // No need to round here + // Calculate the position + int wheelCenterX = backgroundRect.right() - wheel.width() - 17.5; + int wheelCenterY = backgroundRect.height() - wheel.height() + 15; + + // Rotate the wheel image + QTransform rotationTransform; + rotationTransform.translate(wheel.width() / 2.0, wheel.height() / 2.0); + rotationTransform.rotate(steeringAngle); + rotationTransform.translate(-wheel.width() / 2.0, -wheel.height() / 2.0); + + QImage rotatedWheel = wheel.transformed(rotationTransform, Qt::SmoothTransformation); + + QPointF drawPoint( + wheelCenterX - rotatedWheel.width() / 2, wheelCenterY - rotatedWheel.height() / 2); + + // Draw the rotated image + painter.drawImage(drawPoint.x(), drawPoint.y(), rotatedWheel); + + QString steeringAngleStringAfterModulo = QString::number(fmod(steeringAngle, 360), 'f', 0); + + // Draw the steering angle text + QFont steeringFont("Quicksand", 9, QFont::Bold); + painter.setFont(steeringFont); + painter.setPen(QColor(0, 0, 0, 255)); + QRect steeringRect( + wheelCenterX - wheelImage.width() / 2, wheelCenterY - wheelImage.height() / 2, + wheelImage.width(), wheelImage.height()); + painter.drawText(steeringRect, Qt::AlignCenter, steeringAngleStringAfterModulo + "°"); +} + +QImage SteeringWheelDisplay::coloredImage(const QImage & source, const QColor & color) +{ + QImage result = source; + QPainter p(&result); + p.setCompositionMode(QPainter::CompositionMode_SourceAtop); + p.fillRect(result.rect(), color); + p.end(); + return result; +} + +} // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/traffic_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/traffic_display.cpp new file mode 100644 index 0000000000000..233da1f36b4a7 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/traffic_display.cpp @@ -0,0 +1,113 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "traffic_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +TrafficDisplay::TrafficDisplay() +{ + // Load the traffic light image + std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + std::string image_path = package_path + "/assets/images/traffic.png"; + traffic_light_image_.load(image_path.c_str()); +} + +void TrafficDisplay::updateTrafficLightData( + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr & msg) +{ + current_traffic_ = *msg; +} + +void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF & backgroundRect) +{ + // Enable Antialiasing for smoother drawing + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setRenderHint(QPainter::SmoothPixmapTransform, true); + + // Define the area for the circle (background) + QRectF circleRect = backgroundRect; + circleRect.setWidth(backgroundRect.width() / 2 - 20); + circleRect.setHeight(backgroundRect.height() - 20); + circleRect.moveTopRight(QPointF(backgroundRect.right() - 10, backgroundRect.top() + 10)); + + painter.setBrush(QBrush(gray)); + painter.drawEllipse(circleRect.center(), 30, 30); + + // Define the area for the traffic light image (should be smaller or positioned within the circle) + QRectF imageRect = + circleRect.adjusted(15, 15, -15, -15); // Adjusting the rectangle to make the image smaller + + QImage scaled_traffic_image = traffic_light_image_.scaled( + imageRect.size().toSize(), Qt::KeepAspectRatio, Qt::SmoothTransformation); + + if (current_traffic_.signals.size() > 0) { + switch (current_traffic_.signals[0].elements[0].color) { + case 1: + painter.setBrush(QBrush(red)); + painter.drawEllipse(circleRect.center(), 30, 30); + break; + case 2: + painter.setBrush(QBrush(yellow)); + painter.drawEllipse(circleRect.center(), 30, 30); + break; + case 3: + painter.setBrush(QBrush(green)); + painter.drawEllipse(circleRect.center(), 30, 30); + break; + case 4: + painter.setBrush(QBrush(gray)); + painter.drawEllipse(circleRect.center(), 30, 30); + break; + default: + painter.setBrush(QBrush(gray)); + painter.drawEllipse(circleRect.center(), 30, 30); + break; + } + } + // make the image thicker + painter.setPen(QPen(Qt::black, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + + painter.drawImage(imageRect, scaled_traffic_image); +} + +QImage TrafficDisplay::coloredImage(const QImage & source, const QColor & color) +{ + QImage result = source; + QPainter p(&result); + p.setCompositionMode(QPainter::CompositionMode_SourceAtop); + p.fillRect(result.rect(), color); + p.end(); + return result; +} + +} // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/turn_signals_display.cpp b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/turn_signals_display.cpp new file mode 100644 index 0000000000000..a9b780cb4eab2 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/awf_2d_overlay_vehicle/src/turn_signals_display.cpp @@ -0,0 +1,122 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "turn_signals_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace awf_2d_overlay_vehicle +{ + +TurnSignalsDisplay::TurnSignalsDisplay() : current_turn_signal_(0) +{ + last_toggle_time_ = std::chrono::steady_clock::now(); + + // Load the arrow image + std::string package_path = ament_index_cpp::get_package_share_directory("awf_2d_overlay_vehicle"); + std::string image_path = package_path + "/assets/images/arrow.png"; + arrowImage.load(image_path.c_str()); +} + +void TurnSignalsDisplay::updateTurnSignalsData( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) +{ + try { + // Assuming msg->report is the field you're interested in + current_turn_signal_ = msg->report; + } catch (const std::exception & e) { + // Log the error + std::cerr << "Error in processMessage: " << e.what() << std::endl; + } +} + +void TurnSignalsDisplay::updateHazardLightsData( + const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) +{ + try { + // Assuming msg->report is the field you're interested in + current_hazard_lights_ = msg->report; + } catch (const std::exception & e) { + // Log the error + std::cerr << "Error in processMessage: " << e.what() << std::endl; + } +} + +void TurnSignalsDisplay::drawArrows( + QPainter & painter, const QRectF & backgroundRect, const QColor & color) +{ + QImage scaledLeftArrow = arrowImage.scaled(64, 43, Qt::KeepAspectRatio, Qt::SmoothTransformation); + scaledLeftArrow = coloredImage(scaledLeftArrow, gray); + QImage scaledRightArrow = scaledLeftArrow.mirrored(true, false); + int arrowYPos = (backgroundRect.height() / 3 - scaledLeftArrow.height() / 2); + int leftArrowXPos = backgroundRect.width() / 4 - scaledLeftArrow.width(); // Adjust as needed + int rightArrowXPos = backgroundRect.width() * 3 / 4; // Adjust as needed + + bool leftActive = + (current_turn_signal_ == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT || + current_hazard_lights_ == autoware_auto_vehicle_msgs::msg::HazardLightsReport::ENABLE); + bool rightActive = + (current_turn_signal_ == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT || + current_hazard_lights_ == autoware_auto_vehicle_msgs::msg::HazardLightsReport::ENABLE); + + // Color the arrows based on the state of the turn signals and hazard lights by having them blink + // on and off + if (this->blink_on_) { + if (leftActive) { + scaledLeftArrow = coloredImage(scaledLeftArrow, color); + } + if (rightActive) { + scaledRightArrow = coloredImage(scaledRightArrow, color); + } + } + + // Draw the arrows + painter.drawImage(QPointF(leftArrowXPos, arrowYPos), scaledLeftArrow); + painter.drawImage(QPointF(rightArrowXPos, arrowYPos), scaledRightArrow); + + auto now = std::chrono::steady_clock::now(); + if ( + std::chrono::duration_cast(now - last_toggle_time_) >= + blink_interval_) { + blink_on_ = !blink_on_; // Toggle the blink state + last_toggle_time_ = now; + } +} + +QImage TurnSignalsDisplay::coloredImage(const QImage & source, const QColor & color) +{ + QImage result = source; + QPainter p(&result); + p.setCompositionMode(QPainter::CompositionMode_SourceAtop); + p.fillRect(result.rect(), color); + p.end(); + return result; +} + +} // namespace awf_2d_overlay_vehicle diff --git a/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CHANGELOG.rst b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CHANGELOG.rst new file mode 100644 index 0000000000000..e7672ee001955 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CHANGELOG.rst @@ -0,0 +1,24 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rviz_2d_overlay_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.3.0 (2023-05-18) +------------------ +* Removed old position message fields +* Contributors: Dominik, Jonas Otto + +1.2.1 (2022-09-30) +------------------ + +1.2.0 (2022-09-27) +------------------ +* Rename package from overlay_rviz_msgs to rviz_2d_overlay_msgs +* Contributors: Jonas Otto + +1.1.0 (2022-09-11) +------------------ + +1.0.0 (2022-08-30) +------------------ +* Create OverlayText message (currently same as jsk_rviz_plugins) +* Contributors: Jonas Otto, Dominik Authaler diff --git a/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CMakeLists.txt b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CMakeLists.txt new file mode 100644 index 0000000000000..9e9a8f277fd53 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.5) +project(rviz_2d_overlay_msgs) + +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif () + + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/OverlayText.msg" + DEPENDENCIES + std_msgs +) + +ament_package() diff --git a/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/msg/OverlayText.msg b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/msg/OverlayText.msg new file mode 100644 index 0000000000000..db3cf628b895b --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/msg/OverlayText.msg @@ -0,0 +1,31 @@ +uint8 ADD = 0 +uint8 DELETE = 1 + +# constants for the horizontal and vertical alignment +uint8 LEFT = 0 +uint8 RIGHT = 1 +uint8 CENTER = 2 +uint8 TOP = 3 +uint8 BOTTOM = 4 + +uint8 action + +int32 width +int32 height +# Position: Positive values move the overlay towards the center of the window, +# for center alignment positive values move the overlay towards the bottom right +int32 horizontal_distance # Horizontal distance from left/right border or center, depending on alignment +int32 vertical_distance # Vertical distance between from top/bottom border or center, depending on alignment + +# Alignment of the overlay withing RVIZ +uint8 horizontal_alignment # one of LEFT, CENTER, RIGHT +uint8 vertical_alignment # one of TOP, CENTER, BOTTOM + +std_msgs/ColorRGBA bg_color + +int32 line_width +float32 text_size +string font +std_msgs/ColorRGBA fg_color + +string text diff --git a/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/package.xml b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/package.xml new file mode 100644 index 0000000000000..53396c64aa156 --- /dev/null +++ b/common/awf_vehicle_rviz_plugin/rviz_2d_overlay_msgs/package.xml @@ -0,0 +1,24 @@ + + + + rviz_2d_overlay_msgs + 1.3.0 + Messages describing 2D overlays for RVIZ, extracted/derived from the jsk_visualization package. + Team Spatzenhirn + BSD-3-Clause + + ament_cmake + rosidl_default_generators + + autoware_auto_vehicle_msgs + autoware_perception_msgs + std_msgs + unique_identifier_msgs + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/common/object_recognition_utils/package.xml b/common/object_recognition_utils/package.xml index 2f2472515ebad..796e276c376d6 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -6,8 +6,8 @@ The object_recognition_utils package Takayuki Murooka Satoshi Tanaka - Yusuke Muramatsu Shunsuke Miura + Yoshi Ri Apache License 2.0 ament_cmake_auto diff --git a/common/perception_utils/package.xml b/common/perception_utils/package.xml index 5304d481737e5..9d5fbf40a4e89 100644 --- a/common/perception_utils/package.xml +++ b/common/perception_utils/package.xml @@ -5,8 +5,8 @@ 0.1.0 The perception_utils package Satoshi Tanaka - Yusuke Muramatsu Shunsuke Miura + Yoshi Ri Apache License 2.0 ament_cmake_auto diff --git a/common/tensorrt_common/include/tensorrt_common/logger.hpp b/common/tensorrt_common/include/tensorrt_common/logger.hpp index 98fe18794998d..73190d13b9de3 100644 --- a/common/tensorrt_common/include/tensorrt_common/logger.hpp +++ b/common/tensorrt_common/include/tensorrt_common/logger.hpp @@ -19,6 +19,7 @@ #include "NvInferRuntimeCommon.h" +#include #include #include #include @@ -26,6 +27,7 @@ #include #include #include +#include namespace tensorrt_common { @@ -200,7 +202,15 @@ class Logger : public nvinfer1::ILogger // NOLINT public: // Logger(Severity severity = Severity::kWARNING) // Logger(Severity severity = Severity::kVERBOSE) - explicit Logger(Severity severity = Severity::kINFO) : mReportableSeverity(severity) {} + explicit Logger(Severity severity = Severity::kINFO) + : mReportableSeverity(severity), mVerbose(true), mThrottleStopFlag(false) + { + } + + explicit Logger(const bool verbose, Severity severity = Severity::kINFO) + : mReportableSeverity(severity), mVerbose(verbose), mThrottleStopFlag(false) + { + } //! //! \enum TestResult @@ -234,7 +244,44 @@ class Logger : public nvinfer1::ILogger // NOLINT //! void log(Severity severity, const char * msg) noexcept override { - LogStreamConsumer(mReportableSeverity, severity) << "[TRT] " << std::string(msg) << std::endl; + if (mVerbose) { + LogStreamConsumer(mReportableSeverity, severity) << "[TRT] " << std::string(msg) << std::endl; + } + } + + /** + * @brief Logging with throttle. + * + * @example + * Logger logger(); + * auto log_thread = logger.log_throttle(nvinfer1::ILogger::Severity::kINFO, "SOME MSG", 1); + * // some operation + * logger.stop_throttle(log_thread); + * + * @param severity + * @param msg + * @param duration + * @return std::thread + * + */ + std::thread log_throttle(Severity severity, const char * msg, const int duration) noexcept + { + mThrottleStopFlag.store(false); + auto log_func = [this](Severity s, const char * m, const int d) { + while (!mThrottleStopFlag.load()) { + this->log(s, m); + std::this_thread::sleep_for(std::chrono::seconds(d)); + } + }; + + std::thread log_thread(log_func, severity, msg, duration); + return log_thread; + } + + void stop_throttle(std::thread & log_thread) noexcept + { + mThrottleStopFlag.store(true); + log_thread.join(); } //! @@ -430,6 +477,8 @@ class Logger : public nvinfer1::ILogger // NOLINT } Severity mReportableSeverity; + bool mVerbose; + std::atomic mThrottleStopFlag; }; namespace @@ -444,7 +493,7 @@ namespace //! inline LogStreamConsumer LOG_VERBOSE(const Logger & logger) { - return LogStreamConsumer(logger.getReportableSeverity(), Severity::kVERBOSE); + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kVERBOSE) << "[TRT] "; } //! @@ -456,7 +505,7 @@ inline LogStreamConsumer LOG_VERBOSE(const Logger & logger) //! inline LogStreamConsumer LOG_INFO(const Logger & logger) { - return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINFO); + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINFO) << "[TRT] "; } //! @@ -468,7 +517,7 @@ inline LogStreamConsumer LOG_INFO(const Logger & logger) //! inline LogStreamConsumer LOG_WARN(const Logger & logger) { - return LogStreamConsumer(logger.getReportableSeverity(), Severity::kWARNING); + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kWARNING) << "[TRT] "; } //! @@ -480,7 +529,7 @@ inline LogStreamConsumer LOG_WARN(const Logger & logger) //! inline LogStreamConsumer LOG_ERROR(const Logger & logger) { - return LogStreamConsumer(logger.getReportableSeverity(), Severity::kERROR); + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kERROR) << "[TRT] "; } //! @@ -494,7 +543,7 @@ inline LogStreamConsumer LOG_ERROR(const Logger & logger) //! inline LogStreamConsumer LOG_FATAL(const Logger & logger) { - return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINTERNAL_ERROR); + return LogStreamConsumer(logger.getReportableSeverity(), Severity::kINTERNAL_ERROR) << "[TRT] "; } } // anonymous namespace diff --git a/common/tensorrt_common/src/tensorrt_common.cpp b/common/tensorrt_common/src/tensorrt_common.cpp index f1e4835d2ba2d..2b218cd3e49f2 100644 --- a/common/tensorrt_common/src/tensorrt_common.cpp +++ b/common/tensorrt_common/src/tensorrt_common.cpp @@ -186,7 +186,11 @@ void TrtCommon::setup() } else { std::cout << "Building... " << cache_engine_path << std::endl; logger_.log(nvinfer1::ILogger::Severity::kINFO, "Start build engine"); + auto log_thread = logger_.log_throttle( + nvinfer1::ILogger::Severity::kINFO, + "Applying optimizations and building TRT CUDA engine. Please wait for a few minutes...", 5); buildEngineFromOnnx(model_file_path_, cache_engine_path); + logger_.stop_throttle(log_thread); logger_.log(nvinfer1::ILogger::Severity::kINFO, "End build engine"); } engine_path = cache_engine_path; diff --git a/control/control_validator/package.xml b/control/control_validator/package.xml index c46c2d237b605..faf254708ddbb 100644 --- a/control/control_validator/package.xml +++ b/control/control_validator/package.xml @@ -7,6 +7,9 @@ Kyoichi Sugahara Takamasa Horibe Makoto Kurihara + Mamoru Sobue + Takayuki Murooka + Apache License 2.0 Kyoichi Sugahara diff --git a/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt b/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..4c1e8cf58c262 --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_metrics_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets Charts) +set(QT_WIDGETS_LIB Qt5::Widgets) +set(QT_CHARTS_LIB Qt5::Charts) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/metrics_visualize_panel.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_WIDGETS_LIB} + ${QT_CHARTS_LIB} +) + +target_compile_options(${PROJECT_NAME} PUBLIC -Wno-error=deprecated-copy -Wno-error=pedantic) +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + icons + plugins +) diff --git a/evaluator/tier4_metrics_rviz_plugin/README.md b/evaluator/tier4_metrics_rviz_plugin/README.md new file mode 100644 index 0000000000000..be94141254030 --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/README.md @@ -0,0 +1,16 @@ +# tier4_metrics_rviz_plugin + +## Purpose + +This plugin panel to visualize `planning_evaluator` output. + +## Inputs / Outputs + +| Name | Type | Description | +| ---------------------------------------- | --------------------------------------- | ------------------------------------- | +| `/diagnostic/planning_evaluator/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | Subscribe `planning_evaluator` output | + +## HowToUse + +1. Start rviz and select panels/Add new panel. +2. Select MetricsVisualizePanel and press OK. diff --git a/evaluator/tier4_metrics_rviz_plugin/icons/classes/MetricsVisualizePanel.png b/evaluator/tier4_metrics_rviz_plugin/icons/classes/MetricsVisualizePanel.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/evaluator/tier4_metrics_rviz_plugin/icons/classes/MetricsVisualizePanel.png differ diff --git a/evaluator/tier4_metrics_rviz_plugin/package.xml b/evaluator/tier4_metrics_rviz_plugin/package.xml new file mode 100644 index 0000000000000..7bd930f4f3c4c --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/package.xml @@ -0,0 +1,30 @@ + + + + tier4_metrics_rviz_plugin + 0.0.0 + The tier4_metrics_rviz_plugin package + Satoshi OTA + Kyoichi Sugahara + Maxime CLEMENT + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + diagnostic_msgs + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/evaluator/tier4_metrics_rviz_plugin/plugins/plugin_description.xml b/evaluator/tier4_metrics_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..5aca5bd7faa54 --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,5 @@ + + + MetricsVisualizePanel + + diff --git a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp new file mode 100644 index 0000000000000..79da02b9b65c6 --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.cpp @@ -0,0 +1,86 @@ +// Copyright 2024 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// + +#include "metrics_visualize_panel.hpp" + +#include + +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ +MetricsVisualizePanel::MetricsVisualizePanel(QWidget * parent) +: rviz_common::Panel(parent), grid_(new QGridLayout()) +{ + setLayout(grid_); +} + +void MetricsVisualizePanel::onInitialize() +{ + using std::placeholders::_1; + + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + sub_ = raw_node_->create_subscription( + "/diagnostic/planning_evaluator/metrics", rclcpp::QoS{1}, + std::bind(&MetricsVisualizePanel::onMetrics, this, _1)); + + const auto period = std::chrono::milliseconds(static_cast(1e3 / 10)); + timer_ = raw_node_->create_wall_timer(period, [&]() { onTimer(); }); +} + +void MetricsVisualizePanel::onTimer() +{ + std::lock_guard message_lock(mutex_); + + for (auto & [name, metric] : metrics_) { + metric.updateGraph(); + metric.updateTable(); + } +} + +void MetricsVisualizePanel::onMetrics(const DiagnosticArray::ConstSharedPtr msg) +{ + std::lock_guard message_lock(mutex_); + + const auto time = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9; + + constexpr size_t GRAPH_COL_SIZE = 5; + for (size_t i = 0; i < msg->status.size(); ++i) { + const auto & status = msg->status.at(i); + + if (metrics_.count(status.name) == 0) { + auto metric = Metric(status); + metrics_.emplace(status.name, metric); + grid_->addWidget(metric.getTable(), i / GRAPH_COL_SIZE * 2, i % GRAPH_COL_SIZE); + grid_->setRowStretch(i / GRAPH_COL_SIZE * 2, false); + grid_->addWidget(metric.getChartView(), i / GRAPH_COL_SIZE * 2 + 1, i % GRAPH_COL_SIZE); + grid_->setRowStretch(i / GRAPH_COL_SIZE * 2 + 1, true); + grid_->setColumnStretch(i % GRAPH_COL_SIZE, true); + } + + metrics_.at(status.name).updateData(time, status); + } +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::MetricsVisualizePanel, rviz_common::Panel) diff --git a/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp new file mode 100644 index 0000000000000..6708ecd7071e9 --- /dev/null +++ b/evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp @@ -0,0 +1,205 @@ +// Copyright 2024 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// + +#ifndef METRICS_VISUALIZE_PANEL_HPP_ +#define METRICS_VISUALIZE_PANEL_HPP_ + +#ifndef Q_MOC_RUN +#include +#include +#include +#include +#include +#include +#include +#include +#include +#endif + +#include +#include + +#include + +#include +#include +#include + +namespace rviz_plugins +{ + +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; +using diagnostic_msgs::msg::KeyValue; +using QtCharts::QChart; +using QtCharts::QChartView; +using QtCharts::QLineSeries; + +struct Metric +{ +public: + explicit Metric(const DiagnosticStatus & status) : chart(new QChartView), table(new QTableWidget) + { + init(status); + } + + void init(const DiagnosticStatus & status) + { + QStringList header{}; + + { + auto label = new QLabel; + label->setAlignment(Qt::AlignCenter); + label->setText("metric_name"); + labels.emplace("metric_name", label); + + header.push_back(QString::fromStdString(status.name)); + } + + for (const auto & [key, value] : status.values) { + auto label = new QLabel; + label->setAlignment(Qt::AlignCenter); + labels.emplace(key, label); + + auto plot = new QLineSeries; + plot->setName(QString::fromStdString(key)); + plots.emplace(key, plot); + chart->chart()->addSeries(plot); + chart->chart()->createDefaultAxes(); + + header.push_back(QString::fromStdString(key)); + } + + { + chart->chart()->setTitle(QString::fromStdString(status.name)); + chart->chart()->legend()->setVisible(true); + chart->chart()->legend()->detachFromChart(); + chart->setRenderHint(QPainter::Antialiasing); + } + + { + table->setColumnCount(status.values.size() + 1); + table->setHorizontalHeaderLabels(header); + table->verticalHeader()->hide(); + table->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); + table->setRowCount(1); + table->setSizeAdjustPolicy(QAbstractScrollArea::AdjustToContents); + } + } + + void updateData(const double time, const DiagnosticStatus & status) + { + for (const auto & [key, value] : status.values) { + const double data = std::stod(value); + labels.at(key)->setText(QString::fromStdString(toString(data))); + plots.at(key)->append(time, data); + updateMinMax(data); + } + + { + const auto area = chart->chart()->plotArea(); + const auto rect = chart->chart()->legend()->rect(); + chart->chart()->legend()->setGeometry( + QRectF(area.x(), area.y(), area.width(), rect.height())); + chart->chart()->axes(Qt::Horizontal).front()->setRange(time - 100.0, time); + } + + { + table->setCellWidget(0, 0, labels.at("metric_name")); + } + + for (size_t i = 0; i < status.values.size(); ++i) { + table->setCellWidget(0, i + 1, labels.at(status.values.at(i).key)); + } + } + + void updateMinMax(double data) + { + if (data < y_range_min) { + y_range_min = data > 0.0 ? 0.9 * data : 1.1 * data; + chart->chart()->axes(Qt::Vertical).front()->setMin(y_range_min); + } + + if (data > y_range_max) { + y_range_max = data > 0.0 ? 1.1 * data : 0.9 * data; + chart->chart()->axes(Qt::Vertical).front()->setMax(y_range_max); + } + } + + void updateTable() { table->update(); } + + void updateGraph() { chart->update(); } + + QChartView * getChartView() const { return chart; } + + QTableWidget * getTable() const { return table; } + +private: + static std::optional getValue(const DiagnosticStatus & status, std::string && key) + { + const auto itr = std::find_if( + status.values.begin(), status.values.end(), + [&](const auto & value) { return value.key == key; }); + + if (itr == status.values.end()) { + return std::nullopt; + } + + return itr->value; + } + + static std::string toString(const double & value) + { + std::stringstream ss; + ss << std::scientific << std::setprecision(2) << value; + return ss.str(); + } + + QChartView * chart; + QTableWidget * table; + + std::unordered_map labels; + std::unordered_map plots; + + double y_range_min{std::numeric_limits::max()}; + double y_range_max{std::numeric_limits::lowest()}; +}; + +class MetricsVisualizePanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit MetricsVisualizePanel(QWidget * parent = nullptr); + void onInitialize() override; + +private Q_SLOTS: + +private: + rclcpp::Node::SharedPtr raw_node_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Subscription::SharedPtr sub_; + + void onTimer(); + void onMetrics(const DiagnosticArray::ConstSharedPtr msg); + + QGridLayout * grid_; + + std::mutex mutex_; + std::unordered_map metrics_; +}; +} // namespace rviz_plugins + +#endif // METRICS_VISUALIZE_PANEL_HPP_ diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py deleted file mode 100644 index 23bd2fc337c97..0000000000000 --- a/launch/tier4_map_launch/launch/map.launch.py +++ /dev/null @@ -1,223 +0,0 @@ -# Copyright 2021 Tier IV, Inc. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os - -from ament_index_python import get_package_share_directory -import launch -from launch.actions import DeclareLaunchArgument -from launch.actions import GroupAction -from launch.actions import IncludeLaunchDescription -from launch.actions import OpaqueFunction -from launch.actions import SetLaunchConfiguration -from launch.conditions import IfCondition -from launch.conditions import UnlessCondition -from launch.launch_description_sources import AnyLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import ComposableNodeContainer -from launch_ros.actions import Node -from launch_ros.actions import PushRosNamespace -from launch_ros.descriptions import ComposableNode -from launch_ros.substitutions import FindPackageShare -import yaml - - -def launch_setup(context, *args, **kwargs): - lanelet2_map_loader_param_path = LaunchConfiguration("lanelet2_map_loader_param_path").perform( - context - ) - pointcloud_map_loader_param_path = LaunchConfiguration( - "pointcloud_map_loader_param_path" - ).perform(context) - - with open(lanelet2_map_loader_param_path, "r") as f: - lanelet2_map_loader_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(pointcloud_map_loader_param_path, "r") as f: - pointcloud_map_loader_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - map_hash_generator = Node( - package="map_loader", - executable="map_hash_generator", - name="map_hash_generator", - parameters=[ - { - "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"), - "pointcloud_map_path": LaunchConfiguration("pointcloud_map_path"), - } - ], - ) - - lanelet2_map_loader = ComposableNode( - package="map_loader", - plugin="Lanelet2MapLoaderNode", - name="lanelet2_map_loader", - remappings=[ - ("output/lanelet2_map", "vector_map"), - ], - parameters=[ - { - "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"), - }, - lanelet2_map_loader_param, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - lanelet2_map_visualization = ComposableNode( - package="map_loader", - plugin="Lanelet2MapVisualizationNode", - name="lanelet2_map_visualization", - remappings=[ - ("input/lanelet2_map", "vector_map"), - ("output/lanelet2_map_marker", "vector_map_marker"), - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - pointcloud_map_loader = ComposableNode( - package="map_loader", - plugin="PointCloudMapLoaderNode", - name="pointcloud_map_loader", - remappings=[ - ("output/pointcloud_map", "pointcloud_map"), - ("output/pointcloud_map_metadata", "pointcloud_map_metadata"), - ("service/get_partial_pcd_map", "/map/get_partial_pointcloud_map"), - ("service/get_differential_pcd_map", "/map/get_differential_pointcloud_map"), - ("service/get_selected_pcd_map", "/map/get_selected_pointcloud_map"), - ], - parameters=[ - {"pcd_paths_or_directory": ["[", LaunchConfiguration("pointcloud_map_path"), "]"]}, - {"pcd_metadata_path": [LaunchConfiguration("pointcloud_map_metadata_path")]}, - pointcloud_map_loader_param, - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - map_tf_generator = ComposableNode( - package="map_tf_generator", - plugin="VectorMapTFGeneratorNode", - name="vector_map_tf_generator", - parameters=[ - { - "map_frame": "map", - "viewer_frame": "viewer", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - - map_projection_loader_launch_file = os.path.join( - get_package_share_directory("map_projection_loader"), - "launch", - "map_projection_loader.launch.xml", - ) - map_projection_loader = IncludeLaunchDescription( - AnyLaunchDescriptionSource(map_projection_loader_launch_file), - launch_arguments={ - "map_projector_info_path": LaunchConfiguration("map_projector_info_path"), - "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"), - }.items(), - ) - - container = ComposableNodeContainer( - name="map_container", - namespace="", - package="rclcpp_components", - executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=[ - lanelet2_map_loader, - lanelet2_map_visualization, - pointcloud_map_loader, - map_tf_generator, - ], - output="screen", - ) - - group = GroupAction( - [ - PushRosNamespace("map"), - container, - map_hash_generator, - map_projection_loader, - ] - ) - - return [group] - - -def generate_launch_description(): - launch_arguments = [] - - def add_launch_arg(name: str, default_value=None, description=None): - launch_arguments.append( - DeclareLaunchArgument(name, default_value=default_value, description=description) - ) - - add_launch_arg("map_path", "", "path to map directory"), - add_launch_arg( - "lanelet2_map_path", - [LaunchConfiguration("map_path"), "/lanelet2_map.osm"], - "path to lanelet2 map file", - ), - add_launch_arg( - "pointcloud_map_path", - [LaunchConfiguration("map_path"), "/pointcloud_map.pcd"], - "path to pointcloud map file", - ), - add_launch_arg( - "pointcloud_map_metadata_path", - [LaunchConfiguration("map_path"), "/pointcloud_map_metadata.yaml"], - "path to pointcloud map metadata file", - ), - add_launch_arg( - "map_projector_info_path", - [LaunchConfiguration("map_path"), "/map_projector_info.yaml"], - "path to map projector info yaml file", - ), - add_launch_arg( - "lanelet2_map_loader_param_path", - [ - FindPackageShare("map_loader"), - "/config/lanelet2_map_loader.param.yaml", - ], - "path to lanelet2_map_loader param file", - ), - add_launch_arg( - "pointcloud_map_loader_param_path", - None, - "path to pointcloud_map_loader param file", - ), - add_launch_arg("use_intra_process", "false", "use ROS 2 component container communication"), - add_launch_arg("use_multithread", "false", "use multithread"), - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), - ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), - ) - - return launch.LaunchDescription( - launch_arguments - + [ - set_container_executable, - set_container_mt_executable, - ] - + [OpaqueFunction(function=launch_setup)] - ) diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml index 26b2d462dd53f..56ad5b01c5024 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -1,26 +1,66 @@ + + + + + + - - - + + + + + + + - - - - - - - - + + + + + + + + + + + + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index ab9ed65999048..50b73709cc902 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -31,8 +31,6 @@ - - @@ -75,11 +73,10 @@ - - - - - + + + + @@ -177,7 +174,10 @@ - + + + + @@ -236,7 +236,10 @@ - + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml index 26e7af07646cd..b09684281d33a 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml @@ -2,10 +2,9 @@ - - - - + + + @@ -14,33 +13,32 @@ - - + - + - + - + - + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 528038c5158b2..0d517ced97d17 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -14,8 +14,13 @@ - + + + + + + @@ -185,7 +190,7 @@ - + diff --git a/localization/localization_error_monitor/src/diagnostics.cpp b/localization/localization_error_monitor/src/diagnostics.cpp index 0c5a4a7800639..375fccfa06787 100644 --- a/localization/localization_error_monitor/src/diagnostics.cpp +++ b/localization/localization_error_monitor/src/diagnostics.cpp @@ -23,7 +23,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy( diagnostic_msgs::msg::DiagnosticStatus stat; diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "localization_accuracy"; + key_value.key = "localization_error_ellipse"; key_value.value = std::to_string(ellipse_size); stat.values.push_back(key_value); @@ -47,7 +47,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection diagnostic_msgs::msg::DiagnosticStatus stat; diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "localization_accuracy_lateral_direction"; + key_value.key = "localization_error_ellipse_lateral_direction"; key_value.value = std::to_string(ellipse_size); stat.values.push_back(key_value); diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 95a7cebdc01c8..7956a663f92d5 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -31,13 +31,13 @@ One optional function is regularization. Please see the regularization chapter i | `ndt_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | estimated pose with covariance | | `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | | `points_aligned` | `sensor_msgs::msg::PointCloud2` | [debug topic] pointcloud aligned by scan matching | -| `points_aligned_no_ground` | `sensor_msgs::msg::PointCloud2` | [debug topic] de-grounded pointcloud aligned by scan matching | +| `points_aligned_no_ground` | `sensor_msgs::msg::PointCloud2` | [debug topic] no ground pointcloud aligned by scan matching | | `initial_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] initial pose used in scan matching | | `multi_ndt_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] estimated poses from multiple initial poses in real-time covariance estimation | | `multi_initial_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] initial poses for real-time covariance estimation | | `exe_time_ms` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] execution time for scan matching [ms] | | `transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching | -| `no_ground_transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on de-grounded LiDAR scan | +| `no_ground_transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on no ground LiDAR scan | | `iteration_num` | `tier4_debug_msgs::msg::Int32Stamped` | [debug topic] number of scan matching iterations | | `initial_to_result_relative_pose` | `geometry_msgs::msg::PoseStamped` | [debug topic] relative pose between the initial point and the convergence point | | `initial_to_result_distance` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the initial point and the convergence point [m] | @@ -228,21 +228,19 @@ Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample | single file | at once (standard) | | multiple files | dynamically | -## Scan matching score based on de-grounded LiDAR scan +## Scan matching score based on no ground LiDAR scan ### Abstract -This is a function that uses de-grounded LiDAR scan to estimate the scan matching score. This score can reflect the current localization performance more accurately. +This is a function that uses no ground LiDAR scan to estimate the scan matching score. This score can reflect the current localization performance more accurately. [related issue](https://github.com/autowarefoundation/autoware.universe/issues/2044). ### Parameters - - -| Name | Type | Description | -| ------------------------------------- | ------ | ------------------------------------------------------------------------------------- | -| `estimate_scores_for_degrounded_scan` | bool | Flag for using scan matching score based on de-grounded LiDAR scan (FALSE by default) | -| `z_margin_for_ground_removal` | double | Z-value margin for removal ground points | +| Name | Type | Description | +| ------------------------------------- | ------ | ----------------------------------------------------------------------------------- | +| `estimate_scores_by_no_ground_points` | bool | Flag for using scan matching score based on no ground LiDAR scan (FALSE by default) | +| `z_margin_for_ground_removal` | double | Z-value margin for removal ground points | ## 2D real-time covariance estimation diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index a5a4941bfec62..d4c49b7e8eec5 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -95,9 +95,8 @@ # Radius of input LiDAR range (used for diagnostics of dynamic map loading) lidar_radius: 100.0 - # cspell: ignore degrounded - # A flag for using scan matching score based on de-grounded LiDAR scan - estimate_scores_for_degrounded_scan: false + # A flag for using scan matching score based on no ground LiDAR scan + estimate_scores_by_no_ground_points: false # If lidar_point.z - base_link.z <= this threshold , the point will be removed z_margin_for_ground_removal: 0.8 diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index e2503c20aef6e..2883aa761444d 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -220,8 +220,7 @@ class NDTScanMatcher : public rclcpp::Node std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; - // cspell: ignore degrounded - bool estimate_scores_for_degrounded_scan_; + bool estimate_scores_by_no_ground_points_; double z_margin_for_ground_removal_; // The execution time which means probably NDT cannot matches scans properly diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 0382d805b7276..0278a62341981 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -64,7 +64,6 @@ Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes( throw std::runtime_error("Eigen solver failed. Return output_pose_covariance value."); } -// cspell: ignore degrounded NDTScanMatcher::NDTScanMatcher() : Node("ndt_scan_matcher"), tf2_broadcaster_(*this), @@ -160,8 +159,8 @@ NDTScanMatcher::NDTScanMatcher() this->declare_parameter("initial_estimate_particles_num"); n_startup_trials_ = this->declare_parameter("n_startup_trials"); - estimate_scores_for_degrounded_scan_ = - this->declare_parameter("estimate_scores_for_degrounded_scan"); + estimate_scores_by_no_ground_points_ = + this->declare_parameter("estimate_scores_by_no_ground_points"); z_margin_for_ground_removal_ = this->declare_parameter("z_margin_for_ground_removal"); @@ -526,8 +525,8 @@ void NDTScanMatcher::callback_sensor_points( *sensor_points_in_baselink_frame, *sensor_points_in_map_ptr, ndt_result.pose); publish_point_cloud(sensor_ros_time, map_frame_, sensor_points_in_map_ptr); - // whether use de-grounded points calculate score - if (estimate_scores_for_degrounded_scan_) { + // whether use no ground points to calculate score + if (estimate_scores_by_no_ground_points_) { // remove ground pcl::shared_ptr> no_ground_points_in_map_ptr( new pcl::PointCloud); diff --git a/localization/yabloc/yabloc_common/README.md b/localization/yabloc/yabloc_common/README.md index 9ed871c6ac401..6368305bdbfad 100644 --- a/localization/yabloc/yabloc_common/README.md +++ b/localization/yabloc/yabloc_common/README.md @@ -32,11 +32,7 @@ It estimates the height and tilt of the ground from lanelet2. ### Parameters -| Name | Type | Description | -| ----------------- | ---- | -------------------------------------------------------- | -| `force_zero_tilt` | bool | if true, the tilt is always determined to be horizontal. | -| `K` | int | parameter for nearest k search | -| `R` | int | parameter for radius search | +{{ json_to_markdown("localization/yabloc/yabloc_common/schema/ground_server.schema.json") }} ## ll2_decomposer diff --git a/localization/yabloc/yabloc_image_processing/README.md b/localization/yabloc/yabloc_image_processing/README.md index 921a9277a727c..9816a02c48121 100644 --- a/localization/yabloc/yabloc_image_processing/README.md +++ b/localization/yabloc/yabloc_image_processing/README.md @@ -53,15 +53,7 @@ This node extract road surface region by [graph-based-segmentation](https://docs ### Parameters -| Name | Type | Description | -| --------------------------------- | ------ | ------------------------------------------------------------------ | -| `target_height_ratio` | double | height on the image to retrieve the candidate road surface | -| `target_candidate_box_width` | int | size of the square area to search for candidate road surfaces | -| `pickup_additional_graph_segment` | bool | if this is true, additional regions of similar color are retrieved | -| `similarity_score_threshold` | double | threshold for picking up additional areas | -| `sigma` | double | parameters for cv::ximgproc::segmentation | -| `k` | double | parameters for cv::ximgproc::segmentation | -| `min_size` | double | parameters for cv::ximgproc::segmentation | +{{ json_to_markdown("localization/yabloc/yabloc_image_processing/schema/graph_segment.schema.json") }} ## segment_filter @@ -89,7 +81,7 @@ This is a node that integrates the results of graph_segment and lsd to extract r ### Parameters -{{ json_to_markdown("localization/yabloc/yabloc_common/schema/segment_filter.schema.json") }} +{{ json_to_markdown("localization/yabloc/yabloc_image_processing/schema/segment_filter.schema.json") }} ## undistort @@ -120,7 +112,7 @@ This is to avoid redundant decompression within Autoware. ### Parameters -{{ json_to_markdown("localization/yabloc/yabloc_common/schema/undistort.schema.json") }} +{{ json_to_markdown("localization/yabloc/yabloc_image_processing/schema/undistort.schema.json") }} #### about tf_static overriding diff --git a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt index 164f280becae8..91b272b413c4c 100644 --- a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt +++ b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt @@ -14,6 +14,9 @@ find_package(PCL REQUIRED COMPONENTS common kdtree) # Sophus find_package(Sophus REQUIRED) +# OpenCV +find_package(OpenCV REQUIRED) + # =================================================== # Executable # Camera @@ -28,6 +31,7 @@ ament_auto_add_executable(${TARGET} target_include_directories(${TARGET} PUBLIC include) target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) target_link_libraries(${TARGET} ${PCL_LIBRARIES} Sophus::Sophus) +ament_target_dependencies(${TARGET} OpenCV) # =================================================== ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/localization/yabloc/yabloc_pose_initializer/package.xml b/localization/yabloc/yabloc_pose_initializer/package.xml index e9921db50093e..afd0d4aa86bcf 100644 --- a/localization/yabloc/yabloc_pose_initializer/package.xml +++ b/localization/yabloc/yabloc_pose_initializer/package.xml @@ -17,8 +17,10 @@ rosidl_default_generators autoware_auto_mapping_msgs + cv_bridge geometry_msgs lanelet2_extension + libopencv-dev rclcpp sensor_msgs tier4_localization_msgs diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 0103c7d2b74a7..a9b657f843447 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -5,6 +5,7 @@ 0.1.0 The map_loader package Ryohsuke Mitsudome + Yamato Ando Ryu Yamamoto Koji Minoda Masahiro Sakamoto diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp index 5f4b3e311e6e9..d7bd75a1e9f90 100644 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp @@ -90,8 +90,9 @@ std::map PointCloudMapLoaderNode::getPCDMetadata( { std::map pcd_metadata_dict; if (pcd_paths.size() != 1) { - if (!fs::exists(pcd_metadata_path)) { - throw std::runtime_error("PCD metadata file not found: " + pcd_metadata_path); + while (!fs::exists(pcd_metadata_path)) { + RCLCPP_ERROR_STREAM(get_logger(), "PCD metadata file not found: " << pcd_metadata_path); + std::this_thread::sleep_for(std::chrono::seconds(1)); } pcd_metadata_dict = loadPCDMetadata(pcd_metadata_path); diff --git a/mkdocs_macros.py b/mkdocs_macros.py index 56292a815d80d..97f76442be491 100644 --- a/mkdocs_macros.py +++ b/mkdocs_macros.py @@ -42,6 +42,8 @@ def format_param_range(param): def extract_parameter_info(parameters, namespace=""): params = [] for k, v in parameters.items(): + if "$ref" in v.keys(): + continue if v["type"] != "object": param = {} param["Name"] = namespace + k diff --git a/perception/cluster_merger/CMakeLists.txt b/perception/cluster_merger/CMakeLists.txt index 49506f4b439fb..5ad0de572b44f 100644 --- a/perception/cluster_merger/CMakeLists.txt +++ b/perception/cluster_merger/CMakeLists.txt @@ -24,4 +24,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/perception/cluster_merger/config/cluster_merger.param.yaml b/perception/cluster_merger/config/cluster_merger.param.yaml new file mode 100644 index 0000000000000..adca6c203282f --- /dev/null +++ b/perception/cluster_merger/config/cluster_merger.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + output_frame_id: "base_link" diff --git a/perception/cluster_merger/launch/cluster_merger.launch.xml b/perception/cluster_merger/launch/cluster_merger.launch.xml index 1bbd0ebd91e12..f0f90efe051a0 100644 --- a/perception/cluster_merger/launch/cluster_merger.launch.xml +++ b/perception/cluster_merger/launch/cluster_merger.launch.xml @@ -3,12 +3,12 @@ - + - + diff --git a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml index f7a27a52bfa8a..cddee782e8af0 100644 --- a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml +++ b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml @@ -13,3 +13,4 @@ [800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0] using_2d_validator: false + enable_debugger: false diff --git a/perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml b/perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml new file mode 100644 index 0000000000000..fc5c634735e23 --- /dev/null +++ b/perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + enable_debug: false diff --git a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml index 799b605658345..a1d941e66db4b 100644 --- a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml +++ b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml @@ -9,7 +9,6 @@ - diff --git a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml index 04bcbd87172b3..3acb1f2d1907a 100644 --- a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml +++ b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml @@ -7,6 +7,7 @@ + @@ -22,6 +23,6 @@ - + diff --git a/perception/ground_segmentation/CMakeLists.txt b/perception/ground_segmentation/CMakeLists.txt index bdd793896062a..656ffec486c3c 100644 --- a/perception/ground_segmentation/CMakeLists.txt +++ b/perception/ground_segmentation/CMakeLists.txt @@ -89,6 +89,7 @@ if(BUILD_TESTING) target_link_libraries(test_ray_ground_filter ground_segmentation + ${YAML_CPP_LIBRARIES} ) ament_add_ros_isolated_gtest(test_scan_ground_filter diff --git a/perception/ground_segmentation/config/ground_segmentation.param.yaml b/perception/ground_segmentation/config/ground_segmentation.param.yaml index 8d56e12244716..756882391183e 100644 --- a/perception/ground_segmentation/config/ground_segmentation.param.yaml +++ b/perception/ground_segmentation/config/ground_segmentation.param.yaml @@ -29,3 +29,7 @@ gnd_grid_buffer_size: 4 detection_range_z_max: 2.5 elevation_grid_mode: true + low_priority_region_x: -20.0 + center_pcl_shift: 0.0 + radial_divider_angle_deg: 1.0 + use_recheck_ground_cluster: true diff --git a/perception/ground_segmentation/config/ransac_ground_filter.param.yaml b/perception/ground_segmentation/config/ransac_ground_filter.param.yaml new file mode 100644 index 0000000000000..0d4a7d574499c --- /dev/null +++ b/perception/ground_segmentation/config/ransac_ground_filter.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + base_frame: "base_link" + unit_axis: "z" + max_iterations: 1000 + min_trial: 5000 + min_points: 1000 + outlier_threshold: 0.01 + plane_slope_threshold: 10.0 + voxel_size_x: 0.04 + voxel_size_y: 0.04 + voxel_size_z: 0.04 + height_threshold: 0.01 + debug: false diff --git a/perception/ground_segmentation/config/ray_ground_filter.param.yaml b/perception/ground_segmentation/config/ray_ground_filter.param.yaml new file mode 100644 index 0000000000000..5b9e8c06595f7 --- /dev/null +++ b/perception/ground_segmentation/config/ray_ground_filter.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + min_x: -0.01 + max_x: 0.01 + min_y: -0.01 + max_y: 0.01 + use_vehicle_footprint: false + general_max_slope: 8.0 + local_max_slope: 6.0 + initial_max_slope: 3.0 + radial_divider_angle: 1.0 + min_height_threshold: 0.15 + concentric_divider_distance: 0.0 + reclass_distance_threshold: 0.1 diff --git a/perception/ground_segmentation/config/scan_ground_filter.param.yaml b/perception/ground_segmentation/config/scan_ground_filter.param.yaml new file mode 100644 index 0000000000000..bc02a1ab7e6e7 --- /dev/null +++ b/perception/ground_segmentation/config/scan_ground_filter.param.yaml @@ -0,0 +1,17 @@ +/**: + ros__parameters: + global_slope_max_angle_deg: 10.0 + local_slope_max_angle_deg: 13.0 # recommended 30.0 for non elevation_grid_mode + split_points_distance_tolerance: 0.2 + use_virtual_ground_point: True + split_height_distance: 0.2 + non_ground_height_threshold: 0.20 + grid_size_m: 0.1 + grid_mode_switch_radius: 20.0 + gnd_grid_buffer_size: 4 + detection_range_z_max: 2.5 + elevation_grid_mode: true + low_priority_region_x: -20.0 + center_pcl_shift: 0.0 + radial_divider_angle_deg: 1.0 + use_recheck_ground_cluster: true diff --git a/perception/ground_segmentation/launch/ransac_ground_filter.launch.xml b/perception/ground_segmentation/launch/ransac_ground_filter.launch.xml new file mode 100644 index 0000000000000..f8280b774c6b0 --- /dev/null +++ b/perception/ground_segmentation/launch/ransac_ground_filter.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/perception/ground_segmentation/launch/ray_ground_filter.launch.xml b/perception/ground_segmentation/launch/ray_ground_filter.launch.xml new file mode 100644 index 0000000000000..86375c520a426 --- /dev/null +++ b/perception/ground_segmentation/launch/ray_ground_filter.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/perception/ground_segmentation/launch/scan_ground_filter.launch.xml b/perception/ground_segmentation/launch/scan_ground_filter.launch.xml new file mode 100644 index 0000000000000..2a9e4983ecb56 --- /dev/null +++ b/perception/ground_segmentation/launch/scan_ground_filter.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp index ebcb02df2c614..ed5fb6abe7fe7 100644 --- a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp @@ -85,18 +85,18 @@ using pointcloud_preprocessor::get_param; RANSACGroundFilterComponent::RANSACGroundFilterComponent(const rclcpp::NodeOptions & options) : Filter("RANSACGroundFilter", options) { - base_frame_ = declare_parameter("base_frame", "base_link"); - unit_axis_ = declare_parameter("unit_axis", "z"); - max_iterations_ = declare_parameter("max_iterations", 1000); - min_inliers_ = declare_parameter("min_trial", 5000); - min_points_ = declare_parameter("min_points", 1000); - outlier_threshold_ = declare_parameter("outlier_threshold", 0.01); - plane_slope_threshold_ = declare_parameter("plane_slope_threshold", 10.0); - voxel_size_x_ = declare_parameter("voxel_size_x", 0.04); - voxel_size_y_ = declare_parameter("voxel_size_y", 0.04); - voxel_size_z_ = declare_parameter("voxel_size_z", 0.04); - height_threshold_ = declare_parameter("height_threshold", 0.01); - debug_ = declare_parameter("debug", false); + base_frame_ = declare_parameter("base_frame", "base_link"); + unit_axis_ = declare_parameter("unit_axis"); + max_iterations_ = declare_parameter("max_iterations"); + min_inliers_ = declare_parameter("min_trial"); + min_points_ = declare_parameter("min_points"); + outlier_threshold_ = declare_parameter("outlier_threshold"); + plane_slope_threshold_ = declare_parameter("plane_slope_threshold"); + voxel_size_x_ = declare_parameter("voxel_size_x"); + voxel_size_y_ = declare_parameter("voxel_size_y"); + voxel_size_z_ = declare_parameter("voxel_size_z"); + height_threshold_ = declare_parameter("height_threshold"); + debug_ = declare_parameter("debug"); if (unit_axis_ == "x") { unit_vec_ = Eigen::Vector3d::UnitX(); diff --git a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp index 4b2e81d272e30..7bcae47fd9f1f 100644 --- a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp @@ -48,22 +48,22 @@ RayGroundFilterComponent::RayGroundFilterComponent(const rclcpp::NodeOptions & o grid_precision_ = 0.2; ray_ground_filter::generateColors(colors_, color_num_); - min_x_ = declare_parameter("min_x", -0.01); - max_x_ = declare_parameter("max_x", 0.01); - min_y_ = declare_parameter("min_y", -0.01); - max_y_ = declare_parameter("max_y", 0.01); + min_x_ = declare_parameter("min_x"); + max_x_ = declare_parameter("max_x"); + min_y_ = declare_parameter("min_y"); + max_y_ = declare_parameter("max_y"); setVehicleFootprint(min_x_, max_x_, min_y_, max_y_); - use_vehicle_footprint_ = declare_parameter("use_vehicle_footprint", false); + use_vehicle_footprint_ = declare_parameter("use_vehicle_footprint", false); - general_max_slope_ = declare_parameter("general_max_slope", 8.0); - local_max_slope_ = declare_parameter("local_max_slope", 6.0); - initial_max_slope_ = declare_parameter("initial_max_slope", 3.0); - radial_divider_angle_ = declare_parameter("radial_divider_angle", 1.0); - min_height_threshold_ = declare_parameter("min_height_threshold", 0.15); - concentric_divider_distance_ = declare_parameter("concentric_divider_distance", 0.0); - reclass_distance_threshold_ = declare_parameter("reclass_distance_threshold", 0.1); + general_max_slope_ = declare_parameter("general_max_slope"); + local_max_slope_ = declare_parameter("local_max_slope"); + initial_max_slope_ = declare_parameter("initial_max_slope"); + radial_divider_angle_ = declare_parameter("radial_divider_angle"); + min_height_threshold_ = declare_parameter("min_height_threshold"); + concentric_divider_distance_ = declare_parameter("concentric_divider_distance"); + reclass_distance_threshold_ = declare_parameter("reclass_distance_threshold"); } using std::placeholders::_1; diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index 953a9feb4d21e..f164e0102e0e9 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -37,24 +37,22 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & { // set initial parameters { - low_priority_region_x_ = static_cast(declare_parameter("low_priority_region_x", -20.0f)); - detection_range_z_max_ = static_cast(declare_parameter("detection_range_z_max", 2.5f)); - center_pcl_shift_ = static_cast(declare_parameter("center_pcl_shift", 0.0)); - non_ground_height_threshold_ = - static_cast(declare_parameter("non_ground_height_threshold", 0.20)); - grid_mode_switch_radius_ = - static_cast(declare_parameter("grid_mode_switch_radius", 20.0)); - - grid_size_m_ = static_cast(declare_parameter("grid_size_m", 0.5)); - gnd_grid_buffer_size_ = static_cast(declare_parameter("gnd_grid_buffer_size", 4)); - elevation_grid_mode_ = static_cast(declare_parameter("elevation_grid_mode", true)); - global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg", 8.0)); - local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg", 10.0)); - radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg", 1.0)); - split_points_distance_tolerance_ = declare_parameter("split_points_distance_tolerance", 0.2); - split_height_distance_ = declare_parameter("split_height_distance", 0.2); - use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point", true); - use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster", true); + low_priority_region_x_ = declare_parameter("low_priority_region_x"); + detection_range_z_max_ = declare_parameter("detection_range_z_max"); + center_pcl_shift_ = declare_parameter("center_pcl_shift"); + non_ground_height_threshold_ = declare_parameter("non_ground_height_threshold"); + grid_mode_switch_radius_ = declare_parameter("grid_mode_switch_radius"); + + grid_size_m_ = declare_parameter("grid_size_m"); + gnd_grid_buffer_size_ = declare_parameter("gnd_grid_buffer_size"); + elevation_grid_mode_ = declare_parameter("elevation_grid_mode"); + global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg")); + local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg")); + radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg")); + split_points_distance_tolerance_ = declare_parameter("split_points_distance_tolerance"); + split_height_distance_ = declare_parameter("split_height_distance"); + use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point"); + use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster"); radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_); vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo(); diff --git a/perception/ground_segmentation/test/test_ray_ground_filter.cpp b/perception/ground_segmentation/test/test_ray_ground_filter.cpp index af8bb29ab12b7..0c1db47a5ae09 100644 --- a/perception/ground_segmentation/test/test_ray_ground_filter.cpp +++ b/perception/ground_segmentation/test/test_ray_ground_filter.cpp @@ -29,7 +29,7 @@ #include #include #endif - +#include class RayGroundFilterComponentTestSuite : public ::testing::Test { protected: @@ -63,8 +63,25 @@ class RayGroundFilterComponentTest : public ground_segmentation::RayGroundFilter TEST_F(RayGroundFilterComponentTestSuite, TestCase1) { - // read pcd to pointcloud const auto share_dir = ament_index_cpp::get_package_share_directory("ground_segmentation"); + const auto config_path = share_dir + "/config/ray_ground_filter.param.yaml"; + // std::cout << "config_path:" << config_path << std::endl; + YAML::Node config = YAML::LoadFile(config_path); + auto params = config["/**"]["ros__parameters"]; + + double min_x_ = params["min_x"].as(); + double max_x_ = params["max_x"].as(); + double min_y_ = params["min_y"].as(); + double max_y_ = params["max_y"].as(); + bool use_vehicle_footprint_ = params["use_vehicle_footprint"].as(); + double general_max_slope_ = params["general_max_slope"].as(); + double local_max_slope_ = params["local_max_slope"].as(); + double initial_max_slope_ = params["initial_max_slope"].as(); + double radial_divider_angle_ = params["radial_divider_angle"].as(); + double min_height_threshold_ = params["min_height_threshold"].as(); + double concentric_divider_distance_ = params["concentric_divider_distance"].as(); + double reclass_distance_threshold_ = params["reclass_distance_threshold"].as(); + const auto pcd_path = share_dir + "/data/test.pcd"; pcl::PointCloud cloud; pcl::io::loadPCDFile(pcd_path, cloud); @@ -94,10 +111,23 @@ TEST_F(RayGroundFilterComponentTestSuite, TestCase1) rclcpp::NodeOptions node_options; std::vector parameters; + parameters.emplace_back(rclcpp::Parameter("base_frame", "base_link")); - parameters.emplace_back(rclcpp::Parameter("general_max_slope", 2.0)); - parameters.emplace_back(rclcpp::Parameter("local_max_slope", 3.0)); - parameters.emplace_back(rclcpp::Parameter("initial_max_slope", 1.0)); + parameters.emplace_back(rclcpp::Parameter("general_max_slope", general_max_slope_)); + parameters.emplace_back(rclcpp::Parameter("local_max_slope", local_max_slope_)); + parameters.emplace_back(rclcpp::Parameter("initial_max_slope", initial_max_slope_)); + parameters.emplace_back(rclcpp::Parameter("radial_divider_angle", radial_divider_angle_)); + parameters.emplace_back(rclcpp::Parameter("min_height_threshold", min_height_threshold_)); + parameters.emplace_back( + rclcpp::Parameter("concentric_divider_distance", concentric_divider_distance_)); + parameters.emplace_back( + rclcpp::Parameter("reclass_distance_threshold", reclass_distance_threshold_)); + parameters.emplace_back(rclcpp::Parameter("min_x", min_x_)); + parameters.emplace_back(rclcpp::Parameter("max_x", max_x_)); + parameters.emplace_back(rclcpp::Parameter("min_y", min_y_)); + parameters.emplace_back(rclcpp::Parameter("max_y", max_y_)); + parameters.emplace_back(rclcpp::Parameter("use_vehicle_footprint", use_vehicle_footprint_)); + node_options.parameter_overrides(parameters); auto ray_ground_filter_test = std::make_shared(node_options); ray_ground_filter_test->input_pointcloud_pub_->publish(*input_msg_ptr); diff --git a/perception/ground_segmentation/test/test_scan_ground_filter.cpp b/perception/ground_segmentation/test/test_scan_ground_filter.cpp index acc81f8817cd8..9df9c7e9c7433 100644 --- a/perception/ground_segmentation/test/test_scan_ground_filter.cpp +++ b/perception/ground_segmentation/test/test_scan_ground_filter.cpp @@ -38,6 +38,8 @@ class ScanGroundFilterTest : public ::testing::Test { rclcpp::init(0, nullptr); + parse_yaml(); + dummy_node_ = std::make_shared("ScanGroundFilterTest"); input_pointcloud_pub_ = rclcpp::create_publisher( dummy_node_, "/test_scan_ground_filter/input_cloud", 1); @@ -58,6 +60,30 @@ class ScanGroundFilterTest : public ::testing::Test parameters.emplace_back(rclcpp::Parameter("right_overhang", 0.1)); parameters.emplace_back(rclcpp::Parameter("vehicle_height", 2.5)); parameters.emplace_back(rclcpp::Parameter("max_steer_angle", 0.7)); + + parameters.emplace_back( + rclcpp::Parameter("global_slope_max_angle_deg", global_slope_max_angle_deg_)); + parameters.emplace_back( + rclcpp::Parameter("local_slope_max_angle_deg", local_slope_max_angle_deg_)); + parameters.emplace_back( + rclcpp::Parameter("split_points_distance_tolerance", split_points_distance_tolerance_)); + parameters.emplace_back( + rclcpp::Parameter("use_virtual_ground_point", use_virtual_ground_point_)); + parameters.emplace_back(rclcpp::Parameter("split_height_distance", split_height_distance_)); + parameters.emplace_back( + rclcpp::Parameter("non_ground_height_threshold", non_ground_height_threshold_)); + parameters.emplace_back(rclcpp::Parameter("grid_size_m", grid_size_m_)); + parameters.emplace_back(rclcpp::Parameter("grid_mode_switch_radius", grid_mode_switch_radius_)); + parameters.emplace_back(rclcpp::Parameter("gnd_grid_buffer_size", gnd_grid_buffer_size_)); + parameters.emplace_back(rclcpp::Parameter("detection_range_z_max", detection_range_z_max_)); + parameters.emplace_back(rclcpp::Parameter("elevation_grid_mode", elevation_grid_mode_)); + parameters.emplace_back(rclcpp::Parameter("low_priority_region_x", low_priority_region_x_)); + parameters.emplace_back(rclcpp::Parameter("center_pcl_shift", center_pcl_shift_)); + parameters.emplace_back( + rclcpp::Parameter("radial_divider_angle_deg", radial_divider_angle_deg_)); + parameters.emplace_back( + rclcpp::Parameter("use_recheck_ground_cluster", use_recheck_ground_cluster_)); + options.parameter_overrides(parameters); scan_ground_filter_ = std::make_shared(options); @@ -88,8 +114,6 @@ class ScanGroundFilterTest : public ::testing::Test t.transform.rotation.w = q.w(); tf2::doTransform(*origin_input_msg_ptr, *input_msg_ptr_, t); - - parse_yaml(); } ScanGroundFilterTest() {} @@ -113,10 +137,10 @@ class ScanGroundFilterTest : public ::testing::Test void parse_yaml() { const auto share_dir = ament_index_cpp::get_package_share_directory("ground_segmentation"); - const auto config_path = share_dir + "/config/ground_segmentation.param.yaml"; + const auto config_path = share_dir + "/config/scan_ground_filter.param.yaml"; // std::cout << "config_path:" << config_path << std::endl; YAML::Node config = YAML::LoadFile(config_path); - auto params = config["/**"]["ros__parameters"]["common_ground_filter"]["parameters"]; + auto params = config["/**"]["ros__parameters"]; global_slope_max_angle_deg_ = params["global_slope_max_angle_deg"].as(); local_slope_max_angle_deg_ = params["local_slope_max_angle_deg"].as(); split_points_distance_tolerance_ = params["split_points_distance_tolerance"].as(); @@ -127,6 +151,11 @@ class ScanGroundFilterTest : public ::testing::Test gnd_grid_buffer_size_ = params["gnd_grid_buffer_size"].as(); detection_range_z_max_ = params["detection_range_z_max"].as(); elevation_grid_mode_ = params["elevation_grid_mode"].as(); + low_priority_region_x_ = params["low_priority_region_x"].as(); + use_virtual_ground_point_ = params["use_virtual_ground_point"].as(); + center_pcl_shift_ = params["center_pcl_shift"].as(); + radial_divider_angle_deg_ = params["radial_divider_angle_deg"].as(); + use_recheck_ground_cluster_ = params["use_recheck_ground_cluster"].as(); } float global_slope_max_angle_deg_ = 0.0; @@ -139,6 +168,11 @@ class ScanGroundFilterTest : public ::testing::Test uint16_t gnd_grid_buffer_size_ = 0; float detection_range_z_max_ = 0.0; bool elevation_grid_mode_ = false; + float low_priority_region_x_ = 0.0; + bool use_virtual_ground_point_; + float center_pcl_shift_; + float radial_divider_angle_deg_; + bool use_recheck_ground_cluster_; }; TEST_F(ScanGroundFilterTest, TestCase1) @@ -146,27 +180,6 @@ TEST_F(ScanGroundFilterTest, TestCase1) input_pointcloud_pub_->publish(*input_msg_ptr_); sensor_msgs::msg::PointCloud2 out_cloud; - // set filter parameter - scan_ground_filter_->set_parameter( - rclcpp::Parameter("global_slope_max_angle_deg", global_slope_max_angle_deg_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("local_slope_max_angle_deg", local_slope_max_angle_deg_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("split_points_distance_tolerance", split_points_distance_tolerance_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("split_height_distance", split_height_distance_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("non_ground_height_threshold", non_ground_height_threshold_)); - scan_ground_filter_->set_parameter(rclcpp::Parameter("grid_size_m", grid_size_m_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("grid_mode_switch_radius", grid_mode_switch_radius_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("gnd_grid_buffer_size", gnd_grid_buffer_size_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("detection_range_z_max", detection_range_z_max_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("elevation_grid_mode", elevation_grid_mode_)); - filter(out_cloud); output_pointcloud_pub_->publish(out_cloud); diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index 49ff4dafc7900..1322d053b358a 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -5,7 +5,6 @@ 0.1.0 The image_projection_based_fusion package Yukihiro Saito - Yusuke Muramatsu Shunsuke Miura Yoshi Ri badai nguyen diff --git a/perception/lidar_centerpoint/lib/network/network_trt.cpp b/perception/lidar_centerpoint/lib/network/network_trt.cpp index 2d841d22c2eb1..ad1a7ae90630c 100644 --- a/perception/lidar_centerpoint/lib/network/network_trt.cpp +++ b/perception/lidar_centerpoint/lib/network/network_trt.cpp @@ -14,8 +14,6 @@ #include "lidar_centerpoint/network/network_trt.hpp" -#include - namespace centerpoint { bool VoxelEncoderTRT::setProfile( @@ -65,9 +63,8 @@ bool HeadTRT::setProfile( if ( out_name == std::string("heatmap") && network.getOutput(ci)->getDimensions().d[1] != static_cast(out_channel_sizes_[ci])) { - RCLCPP_ERROR( - rclcpp::get_logger("lidar_centerpoint"), - "Expected and actual number of classes do not match"); + tensorrt_common::LOG_ERROR(logger_) + << "Expected and actual number of classes do not match" << std::endl; return false; } auto out_dims = nvinfer1::Dims4( diff --git a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp index 2255df99dbcf4..91fcce9a80f78 100644 --- a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp +++ b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp @@ -14,8 +14,6 @@ #include "lidar_centerpoint/network/tensorrt_wrapper.hpp" -#include - #include #include @@ -42,7 +40,7 @@ bool TensorRTWrapper::init( runtime_ = tensorrt_common::TrtUniquePtr(nvinfer1::createInferRuntime(logger_)); if (!runtime_) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create runtime"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to create runtime" << std::endl; return false; } @@ -51,7 +49,11 @@ bool TensorRTWrapper::init( if (engine_file.is_open()) { success = loadEngine(engine_path); } else { + auto log_thread = logger_.log_throttle( + nvinfer1::ILogger::Severity::kINFO, + "Applying optimizations and building TRT CUDA engine. Please wait a minutes...", 5); success = parseONNX(onnx_path, engine_path, precision); + logger_.stop_throttle(log_thread); } success &= createContext(); @@ -61,15 +63,15 @@ bool TensorRTWrapper::init( bool TensorRTWrapper::createContext() { if (!engine_) { - RCLCPP_ERROR( - rclcpp::get_logger("lidar_centerpoint"), "Failed to create context: Engine was not created"); + tensorrt_common::LOG_ERROR(logger_) + << "Failed to create context: Engine was not created" << std::endl; return false; } context_ = tensorrt_common::TrtUniquePtr(engine_->createExecutionContext()); if (!context_) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create context"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to create context" << std::endl; return false; } @@ -83,14 +85,14 @@ bool TensorRTWrapper::parseONNX( auto builder = tensorrt_common::TrtUniquePtr(nvinfer1::createInferBuilder(logger_)); if (!builder) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create builder"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to create builder" << std::endl; return false; } auto config = tensorrt_common::TrtUniquePtr(builder->createBuilderConfig()); if (!config) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create config"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to create config" << std::endl; return false; } #if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 @@ -100,12 +102,11 @@ bool TensorRTWrapper::parseONNX( #endif if (precision == "fp16") { if (builder->platformHasFastFp16()) { - RCLCPP_INFO(rclcpp::get_logger("lidar_centerpoint"), "Using TensorRT FP16 Inference"); + tensorrt_common::LOG_INFO(logger_) << "Using TensorRT FP16 Inference" << std::endl; config->setFlag(nvinfer1::BuilderFlag::kFP16); } else { - RCLCPP_INFO( - rclcpp::get_logger("lidar_centerpoint"), - "TensorRT FP16 Inference isn't supported in this environment"); + tensorrt_common::LOG_INFO(logger_) + << "TensorRT FP16 Inference isn't supported in this environment" << std::endl; } } @@ -114,7 +115,7 @@ bool TensorRTWrapper::parseONNX( auto network = tensorrt_common::TrtUniquePtr(builder->createNetworkV2(flag)); if (!network) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create network"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to create network" << std::endl; return false; } @@ -123,23 +124,20 @@ bool TensorRTWrapper::parseONNX( parser->parseFromFile(onnx_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); if (!setProfile(*builder, *network, *config)) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to set profile"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to set profile" << std::endl; return false; } - RCLCPP_INFO_STREAM( - rclcpp::get_logger("lidar_centerpoint"), - "Applying optimizations and building TRT CUDA engine (" << onnx_path << ") ..."); plan_ = tensorrt_common::TrtUniquePtr( builder->buildSerializedNetwork(*network, *config)); if (!plan_) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create serialized network"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to create serialized network" << std::endl; return false; } engine_ = tensorrt_common::TrtUniquePtr( runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); if (!engine_) { - RCLCPP_ERROR(rclcpp::get_logger("lidar_centerpoint"), "Failed to create engine"); + tensorrt_common::LOG_ERROR(logger_) << "Failed to create engine" << std::endl; return false; } @@ -148,7 +146,7 @@ bool TensorRTWrapper::parseONNX( bool TensorRTWrapper::saveEngine(const std::string & engine_path) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("lidar_centerpoint"), "Writing to " << engine_path); + tensorrt_common::LOG_INFO(logger_) << "Writing to " << engine_path << std::endl; std::ofstream file(engine_path, std::ios::out | std::ios::binary); file.write(reinterpret_cast(plan_->data()), plan_->size()); return true; @@ -162,7 +160,7 @@ bool TensorRTWrapper::loadEngine(const std::string & engine_path) std::string engine_str = engine_buffer.str(); engine_ = tensorrt_common::TrtUniquePtr(runtime_->deserializeCudaEngine( reinterpret_cast(engine_str.data()), engine_str.size())); - RCLCPP_INFO_STREAM(rclcpp::get_logger("lidar_centerpoint"), "Loaded engine from " << engine_path); + tensorrt_common::LOG_INFO(logger_) << "Loaded engine from " << engine_path << std::endl; return true; } diff --git a/perception/lidar_centerpoint/package.xml b/perception/lidar_centerpoint/package.xml index ffa22964a2df9..fee97a84be917 100644 --- a/perception/lidar_centerpoint/package.xml +++ b/perception/lidar_centerpoint/package.xml @@ -4,7 +4,6 @@ lidar_centerpoint 1.0.0 The lidar_centerpoint package - Yusuke Muramatsu Kenzo Lobos-Tsunekawa Apache License 2.0 diff --git a/perception/object_range_splitter/CMakeLists.txt b/perception/object_range_splitter/CMakeLists.txt index 7b27a344a5e0d..92c6c0668a085 100644 --- a/perception/object_range_splitter/CMakeLists.txt +++ b/perception/object_range_splitter/CMakeLists.txt @@ -16,4 +16,5 @@ rclcpp_components_register_node(object_range_splitter ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/perception/object_range_splitter/config/object_range_splitter.param.yaml b/perception/object_range_splitter/config/object_range_splitter.param.yaml new file mode 100644 index 0000000000000..45bde990029f3 --- /dev/null +++ b/perception/object_range_splitter/config/object_range_splitter.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + split_range: 30.0 diff --git a/perception/object_range_splitter/launch/object_range_splitter.launch.xml b/perception/object_range_splitter/launch/object_range_splitter.launch.xml index 6588c3e71ef73..3f2f3c6ba24c6 100644 --- a/perception/object_range_splitter/launch/object_range_splitter.launch.xml +++ b/perception/object_range_splitter/launch/object_range_splitter.launch.xml @@ -3,12 +3,12 @@ - + - + diff --git a/perception/object_velocity_splitter/CMakeLists.txt b/perception/object_velocity_splitter/CMakeLists.txt index bcc56f522f0a8..0bf6de39f8f3c 100644 --- a/perception/object_velocity_splitter/CMakeLists.txt +++ b/perception/object_velocity_splitter/CMakeLists.txt @@ -37,4 +37,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/perception/object_velocity_splitter/config/object_velocity_splitter.param.yaml b/perception/object_velocity_splitter/config/object_velocity_splitter.param.yaml new file mode 100644 index 0000000000000..adac831aa402c --- /dev/null +++ b/perception/object_velocity_splitter/config/object_velocity_splitter.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + velocity_threshold: 3.0 diff --git a/perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml b/perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml index 42d7f3d61f386..8ab654d4907a4 100644 --- a/perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml +++ b/perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml @@ -5,13 +5,13 @@ - + - + diff --git a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt b/perception/radar_crossing_objects_noise_filter/CMakeLists.txt index 81b1daabaa7ef..6f1270c656bde 100644 --- a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt +++ b/perception/radar_crossing_objects_noise_filter/CMakeLists.txt @@ -37,4 +37,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/perception/radar_crossing_objects_noise_filter/config/radar_crossing_objects_noise_filter.param.yaml b/perception/radar_crossing_objects_noise_filter/config/radar_crossing_objects_noise_filter.param.yaml new file mode 100644 index 0000000000000..f52edf641fb52 --- /dev/null +++ b/perception/radar_crossing_objects_noise_filter/config/radar_crossing_objects_noise_filter.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + angle_threshold: 1.2210 + velocity_threshold: 1.5 diff --git a/perception/radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml b/perception/radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml index 5c414f37a36a4..25856e12c3ba5 100644 --- a/perception/radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml +++ b/perception/radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml @@ -5,15 +5,13 @@ - - + - - + diff --git a/perception/radar_crossing_objects_noise_filter/package.xml b/perception/radar_crossing_objects_noise_filter/package.xml index eb9ea792d54bc..847ed47743c83 100644 --- a/perception/radar_crossing_objects_noise_filter/package.xml +++ b/perception/radar_crossing_objects_noise_filter/package.xml @@ -7,6 +7,8 @@ Sathshi Tanaka Shunsuke Miura Yoshi Ri + Taekjin Lee + Apache License 2.0 Sathshi Tanaka diff --git a/perception/radar_fusion_to_detected_object/package.xml b/perception/radar_fusion_to_detected_object/package.xml index 21641be7b3b7c..4c09f68d88053 100644 --- a/perception/radar_fusion_to_detected_object/package.xml +++ b/perception/radar_fusion_to_detected_object/package.xml @@ -6,6 +6,9 @@ radar_fusion_to_detected_object Satoshi Tanaka Shunsuke Miura + Yoshi Ri + Taekjin Lee + Apache License 2.0 Satoshi Tanaka diff --git a/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml b/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml index 68fca44bcc723..1759c17a5ab6b 100644 --- a/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml +++ b/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml @@ -1,15 +1,12 @@ - - - - + - + diff --git a/perception/radar_object_clustering/package.xml b/perception/radar_object_clustering/package.xml index 49214b429d052..128197d8dfc47 100644 --- a/perception/radar_object_clustering/package.xml +++ b/perception/radar_object_clustering/package.xml @@ -7,6 +7,7 @@ Sathshi Tanaka Shunsuke Miura Yoshi Ri + Taekjin Lee Apache License 2.0 Sathshi Tanaka diff --git a/perception/radar_tracks_msgs_converter/CMakeLists.txt b/perception/radar_tracks_msgs_converter/CMakeLists.txt index 2a7090eebb996..766fb4b939904 100644 --- a/perception/radar_tracks_msgs_converter/CMakeLists.txt +++ b/perception/radar_tracks_msgs_converter/CMakeLists.txt @@ -24,5 +24,6 @@ endif() ## Package ament_auto_package( INSTALL_TO_SHARE + config launch ) diff --git a/perception/radar_tracks_msgs_converter/README.md b/perception/radar_tracks_msgs_converter/README.md index fa08eb08f521a..b55755fb96f92 100644 --- a/perception/radar_tracks_msgs_converter/README.md +++ b/perception/radar_tracks_msgs_converter/README.md @@ -7,31 +7,15 @@ This package converts from [radar_msgs/msg/RadarTracks](https://github.com/ros-p ## Design -### Input / Output +### Background -- Input - - `~/input/radar_objects` (radar_msgs/msg/RadarTracks.msg): Input radar topic - - `~/input/odometry` (nav_msgs/msg/Odometry.msg): Ego vehicle odometry topic -- Output - - `~/output/radar_detected_objects` (autoware_auto_perception_msgs/msg/DetectedObject.idl): The topic converted to Autoware's message. This is used for radar sensor fusion detection and radar detection. - - `~/output/radar_tracked_objects` (autoware_auto_perception_msgs/msg/TrackedObject.idl): The topic converted to Autoware's message. This is used for tracking layer sensor fusion. +Autoware uses [radar_msgs/msg/RadarTracks.msg](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTracks.msg) as radar objects input data. +To use radar objects data for Autoware perception module easily, `radar_tracks_msgs_converter` converts message type from `radar_msgs/msg/RadarTracks.msg` to `autoware_auto_perception_msgs/msg/DetectedObject`. +In addition, because many detection module have an assumption on base_link frame, `radar_tracks_msgs_converter` provide the functions of transform frame_id. -### Parameters - -- `update_rate_hz` (double): The update rate [hz]. - - Default parameter is 20.0 -- `new_frame_id` (string): The header frame of the output topic. - - Default parameter is "base_link" -- `use_twist_compensation` (bool): If the parameter is true, then the twist of the output objects' topic is compensated by ego vehicle motion. - - Default parameter is "true" -- `use_twist_yaw_compensation` (bool): If the parameter is true, then the ego motion compensation will also consider yaw motion of the ego vehicle. - - Default parameter is "false" -- `static_object_speed_threshold` (float): Specify the threshold for static object speed which determines the flag `is_stationary` [m/s]. - - Default parameter is 1.0 - -## Note +### Note -This package convert the label from `radar_msgs/msg/RadarTrack.msg` to Autoware label. +`Radar_tracks_msgs_converter` converts the label from `radar_msgs/msg/RadarTrack.msg` to Autoware label. Label id is defined as below. | | RadarTrack | Autoware | @@ -45,6 +29,54 @@ Label id is defined as below. | BICYCLE | 32006 | 6 | | PEDESTRIAN | 32007 | 7 | -- [radar_msgs/msg/RadarTrack.msg](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTrack.msg): additional vendor-specific classifications are permitted starting from 32000. +Additional vendor-specific classifications are permitted starting from 32000 in [radar_msgs/msg/RadarTrack.msg](https://github.com/ros-perception/radar_msgs/blob/ros2/msg/RadarTrack.msg). +Autoware objects label is defined in [ObjectClassification.idl](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/ObjectClassification.idl) + +## Interface + +### Input + +- `~/input/radar_objects` (`radar_msgs/msg/RadarTracks.msg`) + - Input radar topic +- `~/input/odometry` (`nav_msgs/msg/Odometry.msg`) + - Ego vehicle odometry topic + +### Output + +- `~/output/radar_detected_objects` (`autoware_auto_perception_msgs/msg/DetectedObject.idl`) + - DetectedObject topic converted to Autoware message. + - This is used for radar sensor fusion detection and radar detection. +- `~/output/radar_tracked_objects` (`autoware_auto_perception_msgs/msg/TrackedObject.idl`) + - TrackedObject topic converted to Autoware message. + - This is used for tracking layer sensor fusion. + +### Parameters + +- `update_rate_hz` (double) [hz] + - Default parameter is 20.0 + +This parameter is update rate for the `onTimer` function. +This parameter should be same as the frame rate of input topics. + +- `new_frame_id` (string) + - Default parameter is "base_link" + +This parameter is the header frame_id of the output topic. + +- `use_twist_compensation` (bool) + - Default parameter is "true" + +This parameter is the flag to use the compensation to linear of ego vehicle's twist. +If the parameter is true, then the twist of the output objects' topic is compensated by the ego vehicle linear motion. + +- `use_twist_yaw_compensation` (bool) + - Default parameter is "false" + +This parameter is the flag to use the compensation to yaw rotation of ego vehicle's twist. +If the parameter is true, then the ego motion compensation will also consider yaw motion of the ego vehicle. + +- `static_object_speed_threshold` (float) [m/s] + - Default parameter is 1.0 -- [Autoware objects label](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/ObjectClassification.idl) +This parameter is the threshold to determine the flag `is_stationary`. +If the velocity is lower than this parameter, the flag `is_stationary` of DetectedObject is set to `true` and dealt as a static object. diff --git a/perception/radar_tracks_msgs_converter/config/radar_tracks_msgs_converter.param.yaml b/perception/radar_tracks_msgs_converter/config/radar_tracks_msgs_converter.param.yaml new file mode 100644 index 0000000000000..5a30a6bb4031d --- /dev/null +++ b/perception/radar_tracks_msgs_converter/config/radar_tracks_msgs_converter.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + update_rate_hz: 20.0 + use_twist_compensation: true + use_twist_yaw_compensation: false + static_object_speed_threshold: 1.0 diff --git a/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml b/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml index cc2bb80c6f4f7..1aa4d51703b0c 100644 --- a/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml +++ b/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml @@ -3,19 +3,13 @@ - - - - + - - - - + diff --git a/perception/radar_tracks_msgs_converter/package.xml b/perception/radar_tracks_msgs_converter/package.xml index 4092125cde33f..dbd900d03cbfd 100644 --- a/perception/radar_tracks_msgs_converter/package.xml +++ b/perception/radar_tracks_msgs_converter/package.xml @@ -6,6 +6,9 @@ radar_tracks_msgs_converter Satoshi Tanaka Shunsuke Miura + Yoshi Ri + Taekjin Lee + Apache License 2.0 Satoshi Tanaka diff --git a/perception/simple_object_merger/CMakeLists.txt b/perception/simple_object_merger/CMakeLists.txt index 77b10c0afb87a..10bd0efa7b8fc 100644 --- a/perception/simple_object_merger/CMakeLists.txt +++ b/perception/simple_object_merger/CMakeLists.txt @@ -25,5 +25,6 @@ endif() # Package ament_auto_package( INSTALL_TO_SHARE + config launch ) diff --git a/perception/simple_object_merger/README.md b/perception/simple_object_merger/README.md index 9bcd03e8abfa1..ec3c03424da52 100644 --- a/perception/simple_object_merger/README.md +++ b/perception/simple_object_merger/README.md @@ -1,23 +1,21 @@ # simple_object_merger -This package can merge multiple topics of [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) without low calculation cost. +This package can merge multiple topics of [autoware_auto_perception_msgs/msg/DetectedObject](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs/-/blob/master/autoware_auto_perception_msgs/msg/DetectedObject.idl) with low calculation cost. -## Algorithm +## Design ### Background [Object_merger](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/object_merger) is mainly used for merge process with DetectedObjects. There are 2 characteristics in `Object_merger`. First, `object_merger` solve data association algorithm like Hungarian algorithm for matching problem, but it needs computational cost. Second, `object_merger` can handle only 2 DetectedObjects topics and cannot handle more than 2 topics in one node. To merge 6 DetectedObjects topics, 6 `object_merger` nodes need to stand for now. -So `simple_object_merger` aim to merge multiple DetectedObjects with low calculation cost. +Therefore, `simple_object_merger` aim to merge multiple DetectedObjects with low calculation cost. The package do not use data association algorithm to reduce the computational cost, and it can handle more than 2 topics in one node to prevent launching a large number of nodes. ### Use case -Use case is as below. - - Multiple radar detection -`Simple_object_merger` can be used for multiple radar detection. By combining them into one topic from multiple radar topics with `simple_object_merger`, the pipeline for faraway detection with radar is simpler. +`Simple_object_merger` can be used for multiple radar detection. By combining them into one topic from multiple radar topics, the pipeline for faraway detection with radar can be simpler. ### Limitation @@ -74,5 +72,19 @@ If the time difference between the first topic of `input_topics` and an input to - Default parameter: "[]" This parameter is the name of input topics. -For example, when this packages use for radar objects, `"[/sensing/radar/front_center/detected_objects, /sensing/radar/front_left/detected_objects, /sensing/radar/rear_left/detected_objects, /sensing/radar/rear_center/detected_objects, /sensing/radar/rear_right/detected_objects, /sensing/radar/front_right/detected_objects]"` can be set. +For example, when this packages use for radar objects, + +```yaml +input_topics: + [ + "/sensing/radar/front_center/detected_objects", + "/sensing/radar/front_left/detected_objects", + "/sensing/radar/rear_left/detected_objects", + "/sensing/radar/rear_center/detected_objects", + "/sensing/radar/rear_right/detected_objects", + "/sensing/radar/front_right/detected_objects", + ] +``` + +can be set in config yaml file. For now, the time difference is calculated by the header time between the first topic of `input_topics` and the input topics, so the most important objects to detect should be set in the first of `input_topics` list. diff --git a/perception/simple_object_merger/config/simple_object_merger.param.yaml b/perception/simple_object_merger/config/simple_object_merger.param.yaml new file mode 100644 index 0000000000000..b285a3110d22d --- /dev/null +++ b/perception/simple_object_merger/config/simple_object_merger.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + update_rate_hz: 20.0 + new_frame_id: "base_link" + timeout_threshold: 1.0 + input_topics: [""] diff --git a/perception/simple_object_merger/launch/simple_object_merger.launch.xml b/perception/simple_object_merger/launch/simple_object_merger.launch.xml index 14fca01fa6438..43e22ea081feb 100644 --- a/perception/simple_object_merger/launch/simple_object_merger.launch.xml +++ b/perception/simple_object_merger/launch/simple_object_merger.launch.xml @@ -1,18 +1,9 @@ - - - - - - + - - - - - + diff --git a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp index 693fccb7e937c..683c6921eef5b 100644 --- a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp +++ b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp @@ -82,7 +82,7 @@ SimpleObjectMergerNode::SimpleObjectMergerNode(const rclcpp::NodeOptions & node_ // Node Parameter node_param_.update_rate_hz = declare_parameter("update_rate_hz", 20.0); node_param_.new_frame_id = declare_parameter("new_frame_id", "base_link"); - node_param_.timeout_threshold = declare_parameter("timeout_threshold", 1.0); + node_param_.timeout_threshold = declare_parameter("timeout_threshold", 0.1); declare_parameter("input_topics", std::vector()); node_param_.topic_names = get_parameter("input_topics").as_string_array(); diff --git a/perception/tensorrt_yolo/lib/include/trt_yolo.hpp b/perception/tensorrt_yolo/lib/include/trt_yolo.hpp index af8065ffed379..a64c94c008526 100644 --- a/perception/tensorrt_yolo/lib/include/trt_yolo.hpp +++ b/perception/tensorrt_yolo/lib/include/trt_yolo.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include @@ -67,22 +68,6 @@ struct Deleter template using unique_ptr = std::unique_ptr; -class Logger : public nvinfer1::ILogger -{ -public: - explicit Logger(bool verbose) : verbose_(verbose) {} - - void log(Severity severity, const char * msg) noexcept override - { - if (verbose_ || ((severity != Severity::kINFO) && (severity != Severity::kVERBOSE))) { - std::cout << msg << std::endl; - } - } - -private: - bool verbose_{false}; -}; - struct Config { int num_anchors; @@ -105,7 +90,7 @@ class Net Net( const std::string & onnx_file_path, const std::string & precision, const int max_batch_size, const Config & yolo_config, const std::vector & calibration_images, - const std::string & calibration_table, bool verbose = false, + const std::string & calibration_table, bool verbose = true, size_t workspace_size = (1ULL << 30)); ~Net(); @@ -138,6 +123,7 @@ class Net cuda::unique_ptr out_scores_d_ = nullptr; cuda::unique_ptr out_boxes_d_ = nullptr; cuda::unique_ptr out_classes_d_ = nullptr; + tensorrt_common::Logger logger_; void load(const std::string & path); bool prepare(); diff --git a/perception/tensorrt_yolo/lib/src/trt_yolo.cpp b/perception/tensorrt_yolo/lib/src/trt_yolo.cpp index 7f1d2dbbf4577..c8793aa4c8512 100644 --- a/perception/tensorrt_yolo/lib/src/trt_yolo.cpp +++ b/perception/tensorrt_yolo/lib/src/trt_yolo.cpp @@ -113,10 +113,9 @@ std::vector Net::preprocess( return result; } -Net::Net(const std::string & path, bool verbose) +Net::Net(const std::string & path, bool verbose) : logger_(verbose) { - Logger logger(verbose); - runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger)); + runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger_)); load(path); if (!prepare()) { std::cout << "Fail to prepare engine" << std::endl; @@ -136,9 +135,9 @@ Net::Net( const std::string & onnx_file_path, const std::string & precision, const int max_batch_size, const Config & yolo_config, const std::vector & calibration_images, const std::string & calibration_table, bool verbose, size_t workspace_size) +: logger_(verbose) { - Logger logger(verbose); - runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger)); + runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger_)); if (!runtime_) { std::cout << "Fail to create runtime" << std::endl; return; @@ -147,7 +146,7 @@ Net::Net( bool int8 = precision.compare("INT8") == 0; // Create builder - auto builder = unique_ptr(nvinfer1::createInferBuilder(logger)); + auto builder = unique_ptr(nvinfer1::createInferBuilder(logger_)); if (!builder) { std::cout << "Fail to create builder" << std::endl; return; @@ -168,20 +167,23 @@ Net::Net( #endif // Parse ONNX FCN - std::cout << "Building " << precision << " core model..." << std::endl; + tensorrt_common::LOG_INFO(logger_) << "Building " << precision << " core model..." << std::endl; const auto flag = 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); auto network = unique_ptr(builder->createNetworkV2(flag)); if (!network) { - std::cout << "Fail to create network" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create network" << std::endl; return; } - auto parser = unique_ptr(nvonnxparser::createParser(*network, logger)); + auto parser = unique_ptr(nvonnxparser::createParser(*network, logger_)); if (!parser) { - std::cout << "Fail to create parser" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create parser" << std::endl; return; } + auto log_thread = logger_.log_throttle( + nvinfer1::ILogger::Severity::kINFO, + "Applying optimizations and building TRT CUDA engine. Please wait for a few minutes...", 5); parser->parseFromFile( onnx_file_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); std::vector scores, boxes, classes; @@ -250,28 +252,31 @@ Net::Net( calib = std::make_unique(stream, calibration_table); config->setInt8Calibrator(calib.get()); } else { - std::cout << "Fail to find enough images for INT8 calibration. Build FP mode..." << std::endl; + tensorrt_common::LOG_WARN(logger_) + << "Fail to find enough images for INT8 calibration. Build FP mode..." << std::endl; } } // Build engine - std::cout << "Applying optimizations and building TRT CUDA engine..." << std::endl; plan_ = unique_ptr(builder->buildSerializedNetwork(*network, *config)); if (!plan_) { - std::cout << "Fail to create serialized network" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create serialized network" << std::endl; + logger_.stop_throttle(log_thread); return; } engine_ = unique_ptr( runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); if (!prepare()) { - std::cout << "Fail to create engine" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create engine" << std::endl; + logger_.stop_throttle(log_thread); return; } + logger_.stop_throttle(log_thread); } void Net::save(const std::string & path) const { - std::cout << "Writing to " << path << "..." << std::endl; + tensorrt_common::LOG_INFO(logger_) << "Writing to " << path << "..." << std::endl; std::ofstream file(path, std::ios::out | std::ios::binary); file.write(reinterpret_cast(plan_->data()), plan_->size()); } diff --git a/perception/tensorrt_yolo/package.xml b/perception/tensorrt_yolo/package.xml index 8a6756449b70f..12d07647093b5 100755 --- a/perception/tensorrt_yolo/package.xml +++ b/perception/tensorrt_yolo/package.xml @@ -16,6 +16,7 @@ rclcpp rclcpp_components sensor_msgs + tensorrt_common tier4_perception_msgs ament_lint_auto diff --git a/perception/tracking_object_merger/config/data_association_matrix.param.yaml b/perception/tracking_object_merger/config/data_association_matrix.param.yaml index 702809b3cede1..c232dbde40b51 100644 --- a/perception/tracking_object_merger/config/data_association_matrix.param.yaml +++ b/perception/tracking_object_merger/config/data_association_matrix.param.yaml @@ -3,7 +3,7 @@ lidar-lidar: can_assign_matrix: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN - [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + [1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN 0, 1, 1, 1, 1, 0, 0, 0, #CAR 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK 0, 1, 1, 1, 1, 0, 0, 0, #BUS @@ -59,7 +59,7 @@ lidar-radar: can_assign_matrix: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN - [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + [1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN 0, 1, 1, 1, 1, 0, 0, 0, #CAR 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK 0, 1, 1, 1, 1, 0, 0, 0, #BUS @@ -115,7 +115,7 @@ radar-radar: can_assign_matrix: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN - [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + [1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN 0, 1, 1, 1, 1, 0, 0, 0, #CAR 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK 0, 1, 1, 1, 1, 0, 0, 0, #BUS diff --git a/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml b/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml index 4a108be657a27..92a7fca229818 100644 --- a/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml +++ b/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml @@ -23,4 +23,4 @@ # logging settings enable_logging: false - log_file_path: "/tmp/decorative_tracker_merger.log" + logging_file_path: "association_log.json" diff --git a/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml b/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml index 5affbe474e8ae..52b0841f97e97 100644 --- a/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml +++ b/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml @@ -5,8 +5,6 @@ - - @@ -15,7 +13,5 @@ - - diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp index 81f63538f9e22..85ab105e61038 100644 --- a/perception/tracking_object_merger/src/utils/utils.cpp +++ b/perception/tracking_object_merger/src/utils/utils.cpp @@ -484,9 +484,9 @@ void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & su { if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) { // warning - std::cerr << "[object_tracking_merger]: motion direction is different in " - "updateWholeTrackedObject function." - << std::endl; + // std::cerr << "[object_tracking_merger]: motion direction is different in " + // "updateWholeTrackedObject function." + // << std::endl; } main_obj = sub_obj; } diff --git a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml index 343531f4a00f1..e96f21b8ff01b 100644 --- a/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml +++ b/perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml @@ -17,7 +17,7 @@ - + diff --git a/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml b/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml index 6e32d3410c260..d08378f3dd129 100644 --- a/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml +++ b/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml @@ -1,7 +1,7 @@ - + @@ -12,7 +12,7 @@ - + diff --git a/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml b/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml index 714c4d288b603..ad2851ced39c6 100644 --- a/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml +++ b/perception/traffic_light_ssd_fine_detector/launch/traffic_light_ssd_fine_detector.launch.xml @@ -16,7 +16,7 @@ - + diff --git a/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp b/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp index 52d411c253fe5..00c21cdcaf29e 100644 --- a/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp +++ b/perception/traffic_light_ssd_fine_detector/lib/include/trt_ssd.hpp @@ -15,6 +15,8 @@ #ifndef TRT_SSD_HPP_ #define TRT_SSD_HPP_ +#include + #include <./cuda_runtime.h> #include @@ -39,22 +41,6 @@ struct Deleter template using unique_ptr = std::unique_ptr; -class Logger : public nvinfer1::ILogger -{ -public: - explicit Logger(bool verbose) : verbose_(verbose) {} - - void log(Severity severity, const char * msg) noexcept override - { - if (verbose_ || ((severity != Severity::kINFO) && (severity != Severity::kVERBOSE))) { - std::cout << msg << std::endl; - } - } - -private: - bool verbose_{false}; -}; - struct Shape { int channel, width, height; @@ -78,7 +64,7 @@ class Net // Create engine from serialized onnx model Net( const std::string & onnx_file_path, const std::string & precision, const int max_batch_size, - bool verbose = false, size_t workspace_size = (1ULL << 30)); + bool verbose = true, size_t workspace_size = (1ULL << 30)); ~Net(); @@ -127,6 +113,7 @@ class Net unique_ptr engine_ = nullptr; unique_ptr context_ = nullptr; cudaStream_t stream_ = nullptr; + tensorrt_common::Logger logger_; void load(const std::string & path); void prepare(); diff --git a/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp b/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp index 22aa0f6bf3880..8aceb32d2ec58 100644 --- a/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp +++ b/perception/traffic_light_ssd_fine_detector/lib/src/trt_ssd.cpp @@ -52,10 +52,9 @@ void Net::prepare() cudaStreamCreate(&stream_); } -Net::Net(const std::string & path, bool verbose) +Net::Net(const std::string & path, bool verbose) : logger_(verbose) { - Logger logger(verbose); - runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger)); + runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger_)); load(path); prepare(); } @@ -70,9 +69,9 @@ Net::~Net() Net::Net( const std::string & onnx_file_path, const std::string & precision, const int max_batch_size, bool verbose, size_t workspace_size) +: logger_(verbose) { - Logger logger(verbose); - runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger)); + runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger_)); if (!runtime_) { std::cout << "Fail to create runtime" << std::endl; return; @@ -81,7 +80,7 @@ Net::Net( bool int8 = precision.compare("INT8") == 0; // Create builder - auto builder = unique_ptr(nvinfer1::createInferBuilder(logger)); + auto builder = unique_ptr(nvinfer1::createInferBuilder(logger_)); if (!builder) { std::cout << "Fail to create builder" << std::endl; return; @@ -102,19 +101,22 @@ Net::Net( #endif // Parse ONNX FCN - std::cout << "Building " << precision << " core model..." << std::endl; + tensorrt_common::LOG_INFO(logger_) << "Building " << precision << " core model..." << std::endl; const auto flag = 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); auto network = unique_ptr(builder->createNetworkV2(flag)); if (!network) { - std::cout << "Fail to create network" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create network" << std::endl; return; } - auto parser = unique_ptr(nvonnxparser::createParser(*network, logger)); + auto parser = unique_ptr(nvonnxparser::createParser(*network, logger_)); if (!parser) { - std::cout << "Fail to create parser" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create parser" << std::endl; return; } + auto log_thread = logger_.log_throttle( + nvinfer1::ILogger::Severity::kINFO, + "Applying optimizations and building TRT CUDA engine. Please wait for a few minutes...", 5); parser->parseFromFile( onnx_file_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); @@ -141,28 +143,31 @@ Net::Net( config->addOptimizationProfile(profile); // Build engine - std::cout << "Applying optimizations and building TRT CUDA engine..." << std::endl; plan_ = unique_ptr(builder->buildSerializedNetwork(*network, *config)); if (!plan_) { - std::cout << "Fail to create serialized network" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create serialized network" << std::endl; + logger_.stop_throttle(log_thread); return; } engine_ = unique_ptr( runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); if (!engine_) { - std::cout << "Fail to create engine" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create engine" << std::endl; + logger_.stop_throttle(log_thread); return; } context_ = unique_ptr(engine_->createExecutionContext()); if (!context_) { - std::cout << "Fail to create context" << std::endl; + tensorrt_common::LOG_ERROR(logger_) << "Fail to create context" << std::endl; + logger_.stop_throttle(log_thread); return; } + logger_.stop_throttle(log_thread); } void Net::save(const std::string & path) { - std::cout << "Writing to " << path << "..." << std::endl; + tensorrt_common::LOG_INFO(logger_) << "Writing to " << path << "..." << std::endl; std::ofstream file(path, std::ios::out | std::ios::binary); file.write(reinterpret_cast(plan_->data()), plan_->size()); } diff --git a/perception/traffic_light_ssd_fine_detector/package.xml b/perception/traffic_light_ssd_fine_detector/package.xml index 994f84bacdae4..cd44857e8fe86 100644 --- a/perception/traffic_light_ssd_fine_detector/package.xml +++ b/perception/traffic_light_ssd_fine_detector/package.xml @@ -16,6 +16,7 @@ rclcpp rclcpp_components sensor_msgs + tensorrt_common tier4_debug_msgs tier4_perception_msgs diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 6107314bc2824..32c92b4f4b88b 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -173,11 +173,9 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( const auto shorten_lanes = utils::cutOverlappedLanes(data.reference_path_rough, data.drivable_lanes); data.left_bound = toLineString3d(utils::calcBound( - planner_data_->route_handler, shorten_lanes, avoidance_parameters_->use_hatched_road_markings, - avoidance_parameters_->use_intersection_areas, true)); + data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, true)); data.right_bound = toLineString3d(utils::calcBound( - planner_data_->route_handler, shorten_lanes, avoidance_parameters_->use_hatched_road_markings, - avoidance_parameters_->use_intersection_areas, false)); + data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, false)); // get related objects from dynamic_objects, and then separates them as target objects and non // target objects diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index b300de2236fcf..d9ae9ed623cb3 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -19,6 +19,7 @@ use_opposite_lane: true use_intersection_areas: true use_hatched_road_markings: true + use_freespace_areas: true # for debug publish_debug_marker: false diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 9b5ae3cb4db9e..5e162a17a5064 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -115,6 +115,9 @@ struct AvoidanceParameters // use intersection area for avoidance bool use_intersection_areas{false}; + // use freespace area for avoidance + bool use_freespace_areas{false}; + // consider avoidance return dead line bool enable_dead_line_for_goal{false}; bool enable_dead_line_for_traffic_light{false}; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index 9f2fdf7ab96d9..03902e4328eb2 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -57,6 +57,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.use_intersection_areas = getOrDeclareParameter(*node, ns + "use_intersection_areas"); p.use_hatched_road_markings = getOrDeclareParameter(*node, ns + "use_hatched_road_markings"); + p.use_freespace_areas = getOrDeclareParameter(*node, ns + "use_freespace_areas"); } // target object diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index f088b9228d964..7c55c62bd8cc1 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -557,6 +557,7 @@ MarkerArray createDebugMarkerArray( addObjects(data.other_objects, std::string("TooNearToGoal")); addObjects(data.other_objects, std::string("ParallelToEgoLane")); addObjects(data.other_objects, std::string("MergingToEgoLane")); + addObjects(data.other_objects, std::string("UnstableObject")); } // shift line pre-process diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index bc1d8c6d75424..5172bcbeb626c 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -235,11 +235,13 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat auto tmp_path = getPreviousModuleOutput().path; const auto shorten_lanes = utils::cutOverlappedLanes(tmp_path, data.drivable_lanes); data.left_bound = toLineString3d(utils::calcBound( - planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings, - parameters_->use_intersection_areas, true)); + getPreviousModuleOutput().path, planner_data_, shorten_lanes, + parameters_->use_hatched_road_markings, parameters_->use_intersection_areas, + parameters_->use_freespace_areas, true)); data.right_bound = toLineString3d(utils::calcBound( - planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings, - parameters_->use_intersection_areas, false)); + getPreviousModuleOutput().path, planner_data_, shorten_lanes, + parameters_->use_hatched_road_markings, parameters_->use_intersection_areas, + parameters_->use_freespace_areas, false)); // reference path if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) { @@ -301,8 +303,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( data, parameters_, forward_detection_range + MARGIN, debug); for (const auto & object : object_outside_target_lane.objects) { - ObjectData other_object; - other_object.object = object; + ObjectData other_object = createObjectData(data, object); other_object.reason = "OutOfTargetArea"; data.other_objects.push_back(other_object); } @@ -938,6 +939,8 @@ BehaviorModuleOutput AvoidanceModule::plan() // expand intersection areas current_drivable_area_info.enable_expanding_intersection_areas = parameters_->use_intersection_areas; + // expand freespace areas + current_drivable_area_info.enable_expanding_freespace_areas = parameters_->use_freespace_areas; output.drivable_area_info = utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 3a9b5db283304..7309882a94e16 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -368,13 +368,15 @@ bool isSafetyCheckTargetObjectType( return parameters->object_parameters.at(object_type).is_safety_check_target; } -bool isVehicleTypeObject(const ObjectData & object) +bool isUnknownTypeObject(const ObjectData & object) { const auto object_type = utils::getHighestProbLabel(object.object.classification); + return object_type == ObjectClassification::UNKNOWN; +} - if (object_type == ObjectClassification::UNKNOWN) { - return false; - } +bool isVehicleTypeObject(const ObjectData & object) +{ + const auto object_type = utils::getHighestProbLabel(object.object.classification); if (object_type == ObjectClassification::PEDESTRIAN) { return false; @@ -387,6 +389,14 @@ bool isVehicleTypeObject(const ObjectData & object) return true; } +bool isMovingObject( + const ObjectData & object, const std::shared_ptr & parameters) +{ + const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters->object_parameters.at(object_type); + return object.move_time > object_parameter.moving_time_threshold; +} + bool isWithinCrosswalk( const ObjectData & object, const std::shared_ptr & overall_graphs) @@ -663,9 +673,7 @@ bool isSatisfiedWithCommonCondition( } // Step2. filtered stopped objects. - const auto object_type = utils::getHighestProbLabel(object.object.classification); - const auto object_parameter = parameters->object_parameters.at(object_type); - if (object.move_time > object_parameter.moving_time_threshold) { + if (filtering_utils::isMovingObject(object, parameters)) { object.reason = AvoidanceDebugFactor::MOVING_OBJECT; return false; } @@ -723,6 +731,15 @@ bool isSatisfiedWithNonVehicleCondition( return false; } + // Object is on center line -> ignore. + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + object.to_centerline = + lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { + object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; + return false; + } + return true; } @@ -1626,6 +1643,16 @@ void filterTargetObjects( o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data); o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); + // TODO(Satoshi Ota) parametrize stop time threshold if need. + constexpr double STOP_TIME_THRESHOLD = 3.0; // [s] + if (filtering_utils::isUnknownTypeObject(o)) { + if (o.stop_time < STOP_TIME_THRESHOLD) { + o.reason = "UnstableObject"; + data.other_objects.push_back(o); + continue; + } + } + if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { data.other_objects.push_back(o); continue; @@ -2278,11 +2305,15 @@ TurnSignalInfo calcTurnSignalInfo( return {}; } - const auto left_lanelets = rh->getAllLeftSharedLinestringLanelets(lanelet, true, true); - const auto right_lanelets = rh->getAllRightSharedLinestringLanelets(lanelet, true, true); + const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true); + const auto left_opposite_lanes = rh->getLeftOppositeLanelets(lanelet); + const auto right_same_direction_lane = rh->getRightLanelet(lanelet, true, true); + const auto right_opposite_lanes = rh->getRightOppositeLanelets(lanelet); + const auto has_left_lane = left_same_direction_lane.has_value() || !left_opposite_lanes.empty(); + const auto has_right_lane = + right_same_direction_lane.has_value() || !right_opposite_lanes.empty(); - if (!existShiftSideLane( - start_shift_length, end_shift_length, left_lanelets.empty(), right_lanelets.empty())) { + if (!existShiftSideLane(start_shift_length, end_shift_length, !has_left_lane, !has_right_lane)) { return {}; } diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 51638e5485fe2..71c309581f251 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -419,7 +419,7 @@ class GoalPlannerModule : public SceneModuleInterface bool checkOccupancyGridCollision(const PathWithLaneId & path) const; bool checkObjectsCollision( const PathWithLaneId & path, const double collision_check_margin, - const bool update_debug_data = false) const; + const bool extract_static_objects, const bool update_debug_data = false) const; // goal seach Pose calcRefinedGoal(const Pose & goal_pose) const; @@ -493,6 +493,7 @@ class GoalPlannerModule : public SceneModuleInterface std::optional last_previous_module_output_{}; bool hasPreviousModulePathShapeChanged() const; bool hasDeviatedFromLastPreviousModulePath() const; + bool hasDeviatedFromCurrentPreviousModulePath() const; // timer for generating pull over path candidates in a separate thread void onTimer(); diff --git a/planning/behavior_path_goal_planner_module/package.xml b/planning/behavior_path_goal_planner_module/package.xml index 213c0093b08d9..a3023389cdd32 100644 --- a/planning/behavior_path_goal_planner_module/package.xml +++ b/planning/behavior_path_goal_planner_module/package.xml @@ -11,6 +11,8 @@ Tomoya Kimura Shumpei Wakabayashi Tomohito Ando + Mamoru Sobue + Daniel Sanchez Apache License 2.0 diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index faa9e28e28b3b..3eb973dc9841e 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -161,6 +161,13 @@ bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath() const planner_data_->self_odometry->pose.pose.position)) > 0.3; } +bool GoalPlannerModule::hasDeviatedFromCurrentPreviousModulePath() const +{ + return std::abs(motion_utils::calcLateralOffset( + getPreviousModuleOutput().path.points, + planner_data_->self_odometry->pose.pose.position)) > 0.3; +} + // generate pull over candidate paths void GoalPlannerModule::onTimer() { @@ -179,6 +186,10 @@ void GoalPlannerModule::onTimer() // check if new pull over path candidates are needed to be generated const bool need_update = std::invoke([&]() { + if (hasDeviatedFromCurrentPreviousModulePath()) { + RCLCPP_ERROR(getLogger(), "has deviated from current previous module path"); + return false; + } if (thread_safe_data_.get_pull_over_path_candidates().empty()) { return true; } @@ -624,7 +635,9 @@ bool GoalPlannerModule::canReturnToLaneParking() if ( parameters_->use_object_recognition && - checkObjectsCollision(path, parameters_->object_recognition_collision_check_margin)) { + checkObjectsCollision( + path, parameters_->object_recognition_collision_check_margin, + /*extract_static_objects=*/false)) { return false; } @@ -711,7 +724,9 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri const auto resampled_path = utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5); for (const auto & scale_factor : scale_factors) { - if (!checkObjectsCollision(resampled_path, margin * scale_factor)) { + if (!checkObjectsCollision( + resampled_path, margin * scale_factor, + /*extract_static_objects=*/true)) { return margin * scale_factor; } } @@ -771,8 +786,10 @@ std::optional> GoalPlannerModule::selectP const auto resampled_path = utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5); if ( - parameters_->use_object_recognition && - checkObjectsCollision(resampled_path, collision_check_margin, true /*update_debug_data*/)) { + parameters_->use_object_recognition && checkObjectsCollision( + resampled_path, collision_check_margin, + /*extract_static_objects=*/true, + /*update_debug_data=*/true)) { continue; } @@ -914,6 +931,7 @@ bool GoalPlannerModule::hasNotDecidedPath() const DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const { const auto & prev_status = prev_data_.deciding_path_status; + const bool enable_safety_check = parameters_->safety_check_params.enable_safety_check; // Once this function returns true, it will continue to return true thereafter if (prev_status.first == DecidingPathStatus::DECIDED) { @@ -927,8 +945,7 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const // If it is dangerous against dynamic objects before approval, do not determine the path. // This eliminates a unsafe path to be approved - if ( - parameters_->safety_check_params.enable_safety_check && !isSafePath().first && !isActivated()) { + if (enable_safety_check && !isSafePath().first && !isActivated()) { RCLCPP_DEBUG( getLogger(), "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " @@ -957,13 +974,20 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const const auto parking_path = utils::resamplePathWithSpline(pull_over_path->getParkingPath(), 0.5); const double margin = parameters_->object_recognition_collision_check_margin * hysteresis_factor; - if (checkObjectsCollision(parking_path, margin)) { + if (checkObjectsCollision(parking_path, margin, /*extract_static_objects=*/false)) { RCLCPP_DEBUG( getLogger(), "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects"); return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; } + if (enable_safety_check && !isSafePath().first) { + RCLCPP_DEBUG( + getLogger(), + "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path is not safe against dynamic objects"); + return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; + } + // if enough time has passed since deciding status starts, transition to DECIDED constexpr double check_collision_duration = 1.0; const double elapsed_time_from_deciding = (clock_->now() - prev_status.second).seconds(); @@ -1406,7 +1430,7 @@ bool GoalPlannerModule::isStuck() parameters_->use_object_recognition && checkObjectsCollision( thread_safe_data_.get_pull_over_path()->getCurrentPath(), - parameters_->object_recognition_collision_check_margin)) { + /*extract_static_objects=*/false, parameters_->object_recognition_collision_check_margin)) { return true; } @@ -1519,41 +1543,31 @@ bool GoalPlannerModule::checkOccupancyGridCollision(const PathWithLaneId & path) bool GoalPlannerModule::checkObjectsCollision( const PathWithLaneId & path, const double collision_check_margin, - const bool update_debug_data) const -{ - const auto pull_over_lane_stop_objects = - goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes( - *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length, parameters_->detection_bound_offset, - *(planner_data_->dynamic_object), parameters_->th_moving_object_velocity); + const bool extract_static_objects, const bool update_debug_data) const +{ + const auto target_objects = std::invoke([&]() { + const auto & p = parameters_; + const auto & rh = *(planner_data_->route_handler); + const auto objects = *(planner_data_->dynamic_object); + if (extract_static_objects) { + return goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes( + rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length, + p->detection_bound_offset, objects, p->th_moving_object_velocity); + } + return goal_planner_utils::extractObjectsInExpandedPullOverLanes( + rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length, + p->detection_bound_offset, objects); + }); std::vector obj_polygons; - for (const auto & object : pull_over_lane_stop_objects.objects) { + for (const auto & object : target_objects.objects) { obj_polygons.push_back(tier4_autoware_utils::toPolygon2d(object)); } /* Expand ego collision check polygon - * - `collision_check_margin` in all directions - * - `extra_stopping_margin` in the moving direction - * - `extra_lateral_margin` in external direction of path curve - * - * - * ^ moving direction - * x - * x - * x - * +----------------------+------x--+ - * | | x | - * | +---------------+ | xx | - * | | | | xx | - * | | ego footprint |xxxxx | - * | | | | | - * | +---------------+ | extra_stopping_margin - * | margin | | - * +----------------------+ | - * | extra_lateral_margin | - * +--------------------------------+ - * + * - `collision_check_margin` is added in all directions. + * - `extra_stopping_margin` adds stopping margin under deceleration constraints forward. + * - `extra_lateral_margin` adds the lateral margin on curves. */ std::vector ego_polygons_expanded{}; const auto curvatures = motion_utils::calcCurvature(path.points); @@ -1564,19 +1578,19 @@ bool GoalPlannerModule::checkObjectsCollision( std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_max_extra_stopping_margin); - double extra_lateral_margin = (-1) * curvatures.at(i) * p.point.longitudinal_velocity_mps * - std::abs(p.point.longitudinal_velocity_mps); - extra_lateral_margin = - std::clamp(extra_lateral_margin, -extra_stopping_margin, extra_stopping_margin); + // The square is meant to imply centrifugal force, but it is not a very well-founded formula. + // TODO(kosuke55): It is needed to consider better way because there is an inherently different + // conception of the inside and outside margins. + const double extra_lateral_margin = std::min( + extra_stopping_margin, + std::abs(curvatures.at(i) * std::pow(p.point.longitudinal_velocity_mps, 2))); - const auto lateral_offset_pose = - tier4_autoware_utils::calcOffsetPose(p.point.pose, 0.0, extra_lateral_margin / 2.0, 0.0); const auto ego_polygon = tier4_autoware_utils::toFootprint( - lateral_offset_pose, + p.point.pose, planner_data_->parameters.base_link2front + collision_check_margin + extra_stopping_margin, planner_data_->parameters.base_link2rear + collision_check_margin, planner_data_->parameters.vehicle_width + collision_check_margin * 2.0 + - std::abs(extra_lateral_margin)); + extra_lateral_margin * 2.0); ego_polygons_expanded.push_back(ego_polygon); } diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp index 4e741a2b3db2c..6a9a8c40c01d1 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp @@ -128,7 +128,7 @@ class LaneChangeInterface : public SceneModuleInterface bool canTransitIdleToRunningState() override; - void setObjectDebugVisualization() const; + void updateDebugMarker() const; void updateSteeringFactorPtr(const BehaviorModuleOutput & output); diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index f991ca0d849f0..e4af48a71c8f3 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -53,9 +53,11 @@ class NormalLaneChange : public LaneChangeBase LaneChangePath getLaneChangePath() const override; + BehaviorModuleOutput getTerminalLaneChangePath() const override; + BehaviorModuleOutput generateOutput() override; - void extendOutputDrivableArea(BehaviorModuleOutput & output) override; + void extendOutputDrivableArea(BehaviorModuleOutput & output) const override; void insertStopPoint(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override; @@ -65,7 +67,7 @@ class NormalLaneChange : public LaneChangeBase void resetParameters() override; - TurnSignalInfo updateOutputTurnSignal() override; + TurnSignalInfo updateOutputTurnSignal() const override; bool calcAbortPath() override; @@ -141,7 +143,11 @@ class NormalLaneChange : public LaneChangeBase const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety = true) const override; - TurnSignalInfo calcTurnSignalInfo() override; + std::optional calcTerminalLaneChangePath( + const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) const; + + TurnSignalInfo calcTurnSignalInfo() const override; bool isValidPath(const PathWithLaneId & path) const override; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index 400d5505dc49f..35d018ad3d58c 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -15,6 +15,7 @@ #define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ #include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/debug_structs.hpp" #include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_lane_change_module/utils/utils.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" @@ -36,7 +37,6 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; -using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -66,7 +66,7 @@ class LaneChangeBase virtual BehaviorModuleOutput generateOutput() = 0; - virtual void extendOutputDrivableArea(BehaviorModuleOutput & output) = 0; + virtual void extendOutputDrivableArea(BehaviorModuleOutput & output) const = 0; virtual PathWithLaneId getReferencePath() const = 0; @@ -74,7 +74,7 @@ class LaneChangeBase virtual void resetParameters() = 0; - virtual TurnSignalInfo updateOutputTurnSignal() = 0; + virtual TurnSignalInfo updateOutputTurnSignal() const = 0; virtual bool hasFinishedLaneChange() const = 0; @@ -88,6 +88,8 @@ class LaneChangeBase virtual LaneChangePath getLaneChangePath() const = 0; + virtual BehaviorModuleOutput getTerminalLaneChangePath() const = 0; + virtual bool isEgoOnPreparePhase() const = 0; virtual bool isRequiredStop(const bool is_object_coming_from_rear) = 0; @@ -137,19 +139,7 @@ class LaneChangeBase const LaneChangeStatus & getLaneChangeStatus() const { return status_; } - const LaneChangePaths & getDebugValidPath() const { return debug_valid_path_; } - - const CollisionCheckDebugMap & getDebugData() const { return object_debug_; } - - const CollisionCheckDebugMap & getAfterApprovalDebugData() const - { - return object_debug_after_approval_; - } - - const LaneChangeTargetObjects & getDebugFilteredObjects() const - { - return debug_filtered_objects_; - } + const data::lane_change::Debug & getDebugData() const { return lane_change_debug_; } const Pose & getEgoPose() const { return planner_data_->self_odometry->pose.pose; } @@ -225,7 +215,7 @@ class LaneChangeBase LaneChangePaths * candidate_paths, const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety) const = 0; - virtual TurnSignalInfo calcTurnSignalInfo() = 0; + virtual TurnSignalInfo calcTurnSignalInfo() const = 0; virtual bool isValidPath(const PathWithLaneId & path) const = 0; @@ -257,12 +247,8 @@ class LaneChangeBase Direction direction_{Direction::NONE}; LaneChangeModuleType type_{LaneChangeModuleType::NORMAL}; - mutable LaneChangePaths debug_valid_path_{}; - mutable CollisionCheckDebugMap object_debug_{}; - mutable CollisionCheckDebugMap object_debug_after_approval_{}; - mutable LaneChangeTargetObjects debug_filtered_objects_{}; - mutable double object_debug_lifetime_{0.0}; mutable StopWatch stop_watch_; + mutable data::lane_change::Debug lane_change_debug_; rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr()); mutable rclcpp::Clock clock_{RCL_ROS_TIME}; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp new file mode 100644 index 0000000000000..8ac1ba3909a50 --- /dev/null +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp @@ -0,0 +1,49 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ +#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ + +#include "behavior_path_lane_change_module/utils/data_structs.hpp" +#include "behavior_path_lane_change_module/utils/path.hpp" + +#include + +#include + +namespace behavior_path_planner::data::lane_change +{ +using utils::path_safety_checker::CollisionCheckDebugMap; +struct Debug +{ + std::string module_type; + LaneChangePaths valid_paths; + CollisionCheckDebugMap collision_check_objects; + CollisionCheckDebugMap collision_check_objects_after_approval; + LaneChangeTargetObjects filtered_objects; + double collision_check_object_debug_lifetime{0.0}; + + void reset() + { + valid_paths.clear(); + collision_check_objects.clear(); + collision_check_objects_after_approval.clear(); + filtered_objects.current_lane.clear(); + filtered_objects.target_lane.clear(); + filtered_objects.other_lane.clear(); + collision_check_object_debug_lifetime = 0.0; + } +}; +} // namespace behavior_path_planner::data::lane_change + +#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp index d6deecd4f46fa..427a26e9b4295 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp @@ -15,9 +15,11 @@ #ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ #define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ +#include "behavior_path_lane_change_module/utils/debug_structs.hpp" #include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include #include #include @@ -26,6 +28,7 @@ namespace marker_utils::lane_change_markers { using behavior_path_planner::LaneChangePath; +using behavior_path_planner::data::lane_change::Debug; using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; using visualization_msgs::msg::MarkerArray; MarkerArray showAllValidLaneChangePath( @@ -37,5 +40,6 @@ MarkerArray showFilteredObjects( const ExtendedPredictedObjects & current_lane_objects, const ExtendedPredictedObjects & target_lane_objects, const ExtendedPredictedObjects & other_lane_objects, const std::string & ns); +MarkerArray createDebugMarkerArray(const Debug & debug_data); } // namespace marker_utils::lane_change_markers #endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index 67506326d92aa..e16afcdbf19ec 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -115,6 +115,10 @@ ShiftLine getLaneChangingShiftLine( const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, const PathWithLaneId & reference_path, const double shift_length); +ShiftLine getLaneChangingShiftLine( + const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, + const PathWithLaneId & reference_path, const double shift_length); + PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index c8e1229fe5af5..c8d53d8756cf3 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -70,7 +70,7 @@ bool LaneChangeInterface::isExecutionRequested() const bool LaneChangeInterface::isExecutionReady() const { - return module_type_->isSafe(); + return module_type_->isSafe() && !module_type_->isAbortState(); } void LaneChangeInterface::updateData() @@ -83,7 +83,7 @@ void LaneChangeInterface::updateData() if (isWaitingApproval()) { module_type_->updateLaneChangeStatus(); } - setObjectDebugVisualization(); + updateDebugMarker(); module_type_->updateSpecialData(); module_type_->resetStopPose(); @@ -109,13 +109,23 @@ BehaviorModuleOutput LaneChangeInterface::plan() stop_pose_ = module_type_->getStopPose(); - for (const auto & [uuid, data] : module_type_->getAfterApprovalDebugData()) { + const auto & lane_change_debug = module_type_->getDebugData(); + for (const auto & [uuid, data] : lane_change_debug.collision_check_objects_after_approval) { const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); } updateSteeringFactorPtr(output); - clearWaitingApproval(); + if (module_type_->isAbortState()) { + waitApproval(); + removeRTCStatus(); + const auto candidate = planCandidate(); + path_candidate_ = std::make_shared(candidate.path_candidate); + updateRTCStatus( + candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); + } else { + clearWaitingApproval(); + } return output; } @@ -123,17 +133,15 @@ BehaviorModuleOutput LaneChangeInterface::plan() BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() { *prev_approved_path_ = getPreviousModuleOutput().path; - module_type_->insertStopPoint( - module_type_->getLaneChangeStatus().current_lanes, *prev_approved_path_); BehaviorModuleOutput out; - out.path = *prev_approved_path_; - out.reference_path = getPreviousModuleOutput().reference_path; - out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; - out.drivable_area_info = getPreviousModuleOutput().drivable_area_info; - out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info); + out = module_type_->getTerminalLaneChangePath(); + module_type_->insertStopPoint(module_type_->getLaneChangeStatus().current_lanes, out.path); + out.turn_signal_info = + getCurrentTurnSignalInfo(out.path, getPreviousModuleOutput().turn_signal_info); - for (const auto & [uuid, data] : module_type_->getDebugData()) { + const auto & lane_change_debug = module_type_->getDebugData(); + for (const auto & [uuid, data] : lane_change_debug.collision_check_objects) { const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); } @@ -291,7 +299,7 @@ bool LaneChangeInterface::canTransitFailureState() bool LaneChangeInterface::canTransitIdleToRunningState() { - setObjectDebugVisualization(); + updateDebugMarker(); auto log_debug_throttled = [&](std::string_view message) -> void { RCLCPP_DEBUG(getLogger(), "%s", message.data()); @@ -312,43 +320,15 @@ bool LaneChangeInterface::canTransitIdleToRunningState() return true; } -void LaneChangeInterface::setObjectDebugVisualization() const +void LaneChangeInterface::updateDebugMarker() const { - debug_marker_.markers.clear(); if (!parameters_->publish_debug_marker) { return; } - using marker_utils::showPolygon; - using marker_utils::showPredictedPath; - using marker_utils::showSafetyCheckInfo; - using marker_utils::lane_change_markers::showAllValidLaneChangePath; - using marker_utils::lane_change_markers::showFilteredObjects; - - const auto debug_data = module_type_->getDebugData(); - const auto debug_after_approval = module_type_->getAfterApprovalDebugData(); - const auto debug_valid_path = module_type_->getDebugValidPath(); - const auto debug_filtered_objects = module_type_->getDebugFilteredObjects(); debug_marker_.markers.clear(); - const auto add = [this](const MarkerArray & added) { - tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); - }; - - add(showAllValidLaneChangePath(debug_valid_path, "lane_change_valid_paths")); - add(showFilteredObjects( - debug_filtered_objects.current_lane, debug_filtered_objects.target_lane, - debug_filtered_objects.other_lane, "object_filtered")); - if (!debug_data.empty()) { - add(showSafetyCheckInfo(debug_data, "object_debug_info")); - add(showPredictedPath(debug_data, "ego_predicted_path")); - add(showPolygon(debug_data, "ego_and_target_polygon_relation")); - } - - if (!debug_after_approval.empty()) { - add(showSafetyCheckInfo(debug_after_approval, "object_debug_info_after_approval")); - add(showPredictedPath(debug_after_approval, "ego_predicted_path_after_approval")); - add(showPolygon(debug_after_approval, "ego_and_target_polygon_relation_after_approval")); - } + using marker_utils::lane_change_markers::createDebugMarkerArray; + debug_marker_ = createDebugMarkerArray(module_type_->getDebugData()); } MarkerArray LaneChangeInterface::getModuleVirtualWall() @@ -464,16 +444,13 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( const double buffer = next_lane_change_buffer + min_length_for_turn_signal_activation + base_to_front; const double path_length = motion_utils::calcArcLength(path.points); - const auto & front_point = path.points.front().point.pose.position; const size_t & current_nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - const double length_front_to_ego = motion_utils::calcSignedArcLength( - path.points, front_point, static_cast(0), current_pose.position, - current_nearest_seg_idx); + const double dist_to_terminal = utils::getDistanceToEndOfLane(current_pose, current_lanes); const auto start_pose = motion_utils::calcLongitudinalOffsetPose(path.points, 0, std::max(path_length - buffer, 0.0)); - if (path_length - length_front_to_ego < buffer && start_pose) { + if (dist_to_terminal - base_to_front < buffer && start_pose) { // modify turn signal current_turn_signal_info.desired_start_point = *start_pose; current_turn_signal_info.desired_end_point = lane_change_path.info.lane_changing_end; diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index f5100f16129c2..a5c49c32e0eb6 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -97,7 +97,7 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) lane_change_parameters_->rss_params_for_stuck, is_stuck); } - debug_valid_path_ = valid_paths; + lane_change_debug_.valid_paths = valid_paths; if (valid_paths.empty()) { safe_path.info.current_lanes = current_lanes; @@ -140,6 +140,59 @@ LaneChangePath NormalLaneChange::getLaneChangePath() const return status_.lane_change_path; } +BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const +{ + BehaviorModuleOutput output; + + if (isAbortState() && abort_path_) { + output.path = abort_path_->path; + output.reference_path = prev_module_reference_path_; + output.turn_signal_info = prev_turn_signal_info_; + extendOutputDrivableArea(output); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); + output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + return output; + } + + const auto current_lanes = getCurrentLanes(); + if (current_lanes.empty()) { + RCLCPP_WARN(logger_, "Current lanes not found!!! Something wrong."); + output.path = prev_module_path_; + output.reference_path = prev_module_reference_path_; + output.drivable_area_info = prev_drivable_area_info_; + output.turn_signal_info = prev_turn_signal_info_; + return output; + } + + const auto terminal_path = + calcTerminalLaneChangePath(current_lanes, getLaneChangeLanes(current_lanes, direction_)); + if (!terminal_path) { + RCLCPP_DEBUG(logger_, "Terminal path not found!!!"); + output.path = prev_module_path_; + output.reference_path = prev_module_reference_path_; + output.drivable_area_info = prev_drivable_area_info_; + output.turn_signal_info = prev_turn_signal_info_; + return output; + } + + output.path = terminal_path->path; + output.reference_path = prev_module_reference_path_; + output.turn_signal_info = updateOutputTurnSignal(); + + extendOutputDrivableArea(output); + + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); + output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + + return output; +} + BehaviorModuleOutput NormalLaneChange::generateOutput() { BehaviorModuleOutput output; @@ -183,7 +236,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() return output; } -void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) +void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) const { const auto & dp = planner_data_->drivable_area_expansion_parameters; @@ -421,17 +474,12 @@ void NormalLaneChange::resetParameters() current_lane_change_state_ = LaneChangeStates::Normal; abort_path_ = nullptr; status_ = {}; + lane_change_debug_.reset(); - object_debug_.clear(); - object_debug_after_approval_.clear(); - debug_filtered_objects_.current_lane.clear(); - debug_filtered_objects_.target_lane.clear(); - debug_filtered_objects_.other_lane.clear(); - debug_valid_path_.clear(); RCLCPP_DEBUG(logger_, "reset all flags and debug information."); } -TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() +TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const { TurnSignalInfo turn_signal_info = calcTurnSignalInfo(); const auto [turn_signal_command, distance_to_vehicle_front] = utils::getPathTurnSignal( @@ -840,6 +888,10 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const auto current_pose = getEgoPose(); const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; + const auto shift_intervals = + route_handler.getLateralIntervalsToPreferredLane(current_lanes.back(), direction_); + const double minimum_lane_change_length = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); // Guard if (objects.objects.empty()) { @@ -909,6 +961,24 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( return std::abs(lateral) > (common_parameters.vehicle_width / 2); }; + // ignore object that are ahead of terminal lane change start + { + double distance_to_terminal_from_object = std::numeric_limits::max(); + for (const auto & polygon_p : obj_polygon_outer) { + const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + Pose polygon_pose; + polygon_pose.position = obj_p; + polygon_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation; + const double dist = utils::getDistanceToEndOfLane(polygon_pose, current_lanes); + if (dist < distance_to_terminal_from_object) { + distance_to_terminal_from_object = dist; + } + } + if (minimum_lane_change_length > distance_to_terminal_from_object) { + continue; + } + } + // ignore static object that are behind the ego vehicle if (obj_velocity_norm < 1.0 && max_dist_ego_to_obj < 0.0) { continue; @@ -1112,7 +1182,7 @@ bool NormalLaneChange::getLaneChangePaths( const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety) const { - object_debug_.clear(); + lane_change_debug_.collision_check_objects.clear(); if (current_lanes.empty() || target_lanes.empty()) { RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); return false; @@ -1158,7 +1228,7 @@ bool NormalLaneChange::getLaneChangePaths( } const auto target_objects = getTargetObjects(current_lanes, target_lanes); - debug_filtered_objects_ = target_objects; + lane_change_debug_.filtered_objects = target_objects; const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); @@ -1385,9 +1455,10 @@ bool NormalLaneChange::getLaneChangePaths( std::vector filtered_objects = filterObjectsInTargetLane(target_objects, target_lanes); if ( - !is_stuck && utils::lane_change::passParkedObject( - route_handler, *candidate_path, filtered_objects, lane_change_buffer, - is_goal_in_route, *lane_change_parameters_, object_debug_)) { + !is_stuck && + utils::lane_change::passParkedObject( + route_handler, *candidate_path, filtered_objects, lane_change_buffer, is_goal_in_route, + *lane_change_parameters_, lane_change_debug_.collision_check_objects)) { debug_print( "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " "lane change."); @@ -1400,7 +1471,8 @@ bool NormalLaneChange::getLaneChangePaths( } const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe( - *candidate_path, target_objects, rss_params, is_stuck, object_debug_); + *candidate_path, target_objects, rss_params, is_stuck, + lane_change_debug_.collision_check_objects); if (is_safe) { debug_print("ACCEPT!!!: it is valid and safe!"); @@ -1416,6 +1488,151 @@ bool NormalLaneChange::getLaneChangePaths( return false; } +std::optional NormalLaneChange::calcTerminalLaneChangePath( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +{ + if (current_lanes.empty() || target_lanes.empty()) { + RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); + return {}; + } + const auto & route_handler = *getRouteHandler(); + const auto & common_parameters = planner_data_->parameters; + + const auto forward_path_length = common_parameters.forward_path_length; + const auto minimum_lane_changing_velocity = + lane_change_parameters_->minimum_lane_changing_velocity; + + const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); + + const double lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); + const double next_lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); + + const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); + + const auto sorted_lane_ids = + utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); + + const auto target_neighbor_preferred_lane_poly_2d = + utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); + if (target_neighbor_preferred_lane_poly_2d.empty()) { + RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); + return {}; + } + + // lane changing start getEgoPose() is at the end of prepare segment + const auto current_lane_terminal_point = + lanelet::utils::conversion::toGeomMsgPt(current_lanes.back().centerline3d().back()); + + double distance_to_terminal_from_goal = 0; + if (is_goal_in_route) { + distance_to_terminal_from_goal = + utils::getDistanceToEndOfLane(route_handler.getGoalPose(), current_lanes); + } + + const auto lane_changing_start_pose = motion_utils::calcLongitudinalOffsetPose( + prev_module_path_.points, current_lane_terminal_point, + -(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal)); + + if (!lane_changing_start_pose) { + RCLCPP_DEBUG(logger_, "Reject: lane changing start pose not found!!!"); + return {}; + } + + const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( + current_lanes, target_lanes.front(), lane_changing_start_pose.value()); + + // Check if the lane changing start point is not on the lanes next to target lanes, + if (target_length_from_lane_change_start_pose > 0.0) { + RCLCPP_DEBUG(logger_, "lane change start getEgoPose() is behind target lanelet!"); + return {}; + } + + const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet( + target_lanes, lane_changing_start_pose.value()); + + const auto [min_lateral_acc, max_lateral_acc] = + lane_change_parameters_->lane_change_lat_acc_map.find(minimum_lane_changing_velocity); + + const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( + shift_length, lane_change_parameters_->lane_changing_lateral_jerk, max_lateral_acc); + + const auto target_segment = getTargetSegment( + target_lanes, lane_changing_start_pose.value(), target_lane_length, lane_change_buffer, + minimum_lane_changing_velocity, next_lane_change_buffer); + + if (target_segment.points.empty()) { + RCLCPP_DEBUG(logger_, "Reject: target segment is empty!! something wrong..."); + return {}; + } + + const lanelet::BasicPoint2d lc_start_point( + lane_changing_start_pose->position.x, lane_changing_start_pose->position.y); + + const auto target_lane_polygon = + lanelet::utils::getPolygonFromArcLength(target_lanes, 0, std::numeric_limits::max()); + const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon(); + + const auto is_valid_start_point = + boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || + boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); + + LaneChangeInfo lane_change_info; + lane_change_info.longitudinal_acceleration = LaneChangePhaseInfo{0.0, 0.0}; + lane_change_info.duration = LaneChangePhaseInfo{0.0, lane_changing_time}; + lane_change_info.velocity = + LaneChangePhaseInfo{minimum_lane_changing_velocity, minimum_lane_changing_velocity}; + lane_change_info.length = LaneChangePhaseInfo{0.0, lane_change_buffer}; + lane_change_info.current_lanes = current_lanes; + lane_change_info.target_lanes = target_lanes; + lane_change_info.lane_changing_start = lane_changing_start_pose.value(); + lane_change_info.lane_changing_end = target_segment.points.front().point.pose; + lane_change_info.lateral_acceleration = max_lateral_acc; + lane_change_info.terminal_lane_changing_velocity = minimum_lane_changing_velocity; + + if (!is_valid_start_point) { + RCLCPP_DEBUG( + logger_, + "Reject: lane changing points are not inside of the target preferred lanes or its " + "neighbors"); + return {}; + } + + const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( + lane_change_buffer, minimum_lane_changing_velocity); + const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( + route_handler, target_lanes, lane_changing_start_pose.value(), target_lane_length, + lane_change_buffer, forward_path_length, resample_interval, is_goal_in_route, + next_lane_change_buffer); + + if (target_lane_reference_path.points.empty()) { + RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!"); + return {}; + } + + lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( + lane_changing_start_pose.value(), target_segment.points.front().point.pose, + target_lane_reference_path, shift_length); + + auto reference_segment = prev_module_path_; + const double length_to_lane_changing_start = motion_utils::calcSignedArcLength( + reference_segment.points, reference_segment.points.front().point.pose.position, + lane_changing_start_pose->position); + utils::clipPathLength(reference_segment, 0, length_to_lane_changing_start, 0.0); + // remove terminal points because utils::clipPathLength() calculates extra long path + reference_segment.points.pop_back(); + reference_segment.points.back().point.longitudinal_velocity_mps = minimum_lane_changing_velocity; + + const auto terminal_lane_change_path = utils::lane_change::constructCandidatePath( + lane_change_info, reference_segment, target_segment, target_lane_reference_path, + sorted_lane_ids); + + return terminal_lane_change_path; +} + PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const { const auto & path = status_.lane_change_path; @@ -1423,7 +1640,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const const auto & target_lanes = status_.target_lanes; const auto target_objects = getTargetObjects(current_lanes, target_lanes); - debug_filtered_objects_ = target_objects; + lane_change_debug_.filtered_objects = target_objects; CollisionCheckDebugMap debug_data; const bool is_stuck = isVehicleStuck(current_lanes); @@ -1431,23 +1648,24 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const path, target_objects, lane_change_parameters_->rss_params_for_abort, is_stuck, debug_data); { // only for debug purpose - object_debug_.clear(); - object_debug_lifetime_ += (stop_watch_.toc(getModuleTypeStr()) / 1000); - if (object_debug_lifetime_ > 2.0) { + lane_change_debug_.collision_check_objects.clear(); + lane_change_debug_.collision_check_object_debug_lifetime += + (stop_watch_.toc(getModuleTypeStr()) / 1000); + if (lane_change_debug_.collision_check_object_debug_lifetime > 2.0) { stop_watch_.toc(getModuleTypeStr(), true); - object_debug_lifetime_ = 0.0; - object_debug_after_approval_.clear(); + lane_change_debug_.collision_check_object_debug_lifetime = 0.0; + lane_change_debug_.collision_check_objects_after_approval.clear(); } if (!safety_status.is_safe) { - object_debug_after_approval_ = debug_data; + lane_change_debug_.collision_check_objects_after_approval = debug_data; } } return safety_status; } -TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() +TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() const { const auto get_blinker_pose = [](const PathWithLaneId & path, const double length) { double accumulated_length = 0.0; @@ -1467,6 +1685,10 @@ TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() TurnSignalInfo turn_signal_info{}; + if (path.path.points.empty()) { + return turn_signal_info; + } + // desired start pose = prepare start pose turn_signal_info.desired_start_point = std::invoke([&]() { const auto blinker_start_duration = planner_data_->parameters.turn_signal_search_time; @@ -1646,31 +1868,25 @@ bool NormalLaneChange::calcAbortPath() ShiftedPath shifted_path; // offset front side - // bool offset_back = false; if (!path_shifter.generate(&shifted_path)) { RCLCPP_ERROR(logger_, "failed to generate abort shifted path."); } - const auto arc_position = lanelet::utils::getArcCoordinates( - reference_lanelets, shifted_path.path.points.at(abort_return_idx).point.pose); - const PathWithLaneId reference_lane_segment = std::invoke([&]() { - const double s_start = arc_position.length; - double s_end = std::max(lanelet::utils::getLaneletLength2d(reference_lanelets), s_start); - - if ( - !reference_lanelets.empty() && - route_handler->isInGoalRouteSection(reference_lanelets.back())) { - const auto goal_arc_coordinates = - lanelet::utils::getArcCoordinates(reference_lanelets, route_handler->getGoalPose()); - const double forward_length = std::max(goal_arc_coordinates.length, s_start); - s_end = std::min(s_end, forward_length); + PathWithLaneId reference_lane_segment = prev_module_path_; + { + const auto terminal_path = + calcTerminalLaneChangePath(reference_lanelets, selected_path.info.target_lanes); + if (terminal_path) { + reference_lane_segment = terminal_path->path; } - PathWithLaneId ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end, true); - ref.points.back().point.longitudinal_velocity_mps = std::min( - ref.points.back().point.longitudinal_velocity_mps, - static_cast(lane_change_parameters_->minimum_lane_changing_velocity)); - return ref; - }); + const auto return_pose = shifted_path.path.points.at(abort_return_idx).point.pose; + const auto seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + reference_lane_segment.points, return_pose, common_param.ego_nearest_dist_threshold, + common_param.ego_nearest_yaw_threshold); + reference_lane_segment.points = motion_utils::cropPoints( + reference_lane_segment.points, return_pose.position, seg_idx, + common_param.forward_path_length, 0.0); + } auto abort_path = selected_path; abort_path.shifted_path = shifted_path; @@ -1802,7 +2018,7 @@ bool NormalLaneChange::isVehicleStuck( using lanelet::utils::getArcCoordinates; const auto base_distance = getArcCoordinates(current_lanes, getEgoPose()).length; - for (const auto & object : debug_filtered_objects_.current_lane) { + for (const auto & object : lane_change_debug_.filtered_objects.current_lane) { const auto & p = object.initial_pose.pose; // TODO(Horibe): consider footprint point // Note: it needs chattering prevention. diff --git a/planning/behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_lane_change_module/src/utils/markers.cpp index 719acba405a68..523b770734bc5 100644 --- a/planning/behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/markers.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -120,4 +121,47 @@ MarkerArray showFilteredObjects( marker_array.markers.end(), other_marker.markers.begin(), other_marker.markers.end()); return marker_array; } + +MarkerArray createDebugMarkerArray(const Debug & debug_data) +{ + using marker_utils::showPolygon; + using marker_utils::showPredictedPath; + using marker_utils::showSafetyCheckInfo; + using marker_utils::lane_change_markers::showAllValidLaneChangePath; + using marker_utils::lane_change_markers::showFilteredObjects; + + const auto & debug_collision_check_object = debug_data.collision_check_objects; + const auto & debug_collision_check_object_after_approval = + debug_data.collision_check_objects_after_approval; + const auto & debug_valid_paths = debug_data.valid_paths; + const auto & debug_filtered_objects = debug_data.filtered_objects; + + MarkerArray debug_marker; + const auto add = [&debug_marker](const MarkerArray & added) { + tier4_autoware_utils::appendMarkerArray(added, &debug_marker); + }; + + add(showAllValidLaneChangePath(debug_valid_paths, "lane_change_valid_paths")); + add(showFilteredObjects( + debug_filtered_objects.current_lane, debug_filtered_objects.target_lane, + debug_filtered_objects.other_lane, "object_filtered")); + + if (!debug_collision_check_object.empty()) { + add(showSafetyCheckInfo(debug_collision_check_object, "collision_check_object_info")); + add(showPredictedPath(debug_collision_check_object, "ego_predicted_path")); + add(showPolygon(debug_collision_check_object, "ego_and_target_polygon_relation")); + } + + if (!debug_collision_check_object_after_approval.empty()) { + add(showSafetyCheckInfo( + debug_collision_check_object_after_approval, "object_debug_info_after_approval")); + add(showPredictedPath( + debug_collision_check_object_after_approval, "ego_predicted_path_after_approval")); + add(showPolygon( + debug_collision_check_object_after_approval, + "ego_and_target_polygon_relation_after_approval")); + } + + return debug_marker; +} } // namespace marker_utils::lane_change_markers diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index df73990b770a3..e6a162d2bdad5 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -412,10 +412,6 @@ PathWithLaneId getReferencePathFromTargetLane( return std::min(dist_from_lc_start, target_lane_length - next_lane_change_buffer); }); - if (s_end - s_start < lane_changing_length) { - return PathWithLaneId(); - } - RCLCPP_DEBUG( rclcpp::get_logger("behavior_path_planner") .get_child("lane_change") @@ -423,6 +419,10 @@ PathWithLaneId getReferencePathFromTargetLane( .get_child("getReferencePathFromTargetLane"), "start: %f, end: %f", s_start, s_end); + if (s_end - s_start < lane_changing_length) { + return PathWithLaneId(); + } + const auto lane_changing_reference_path = route_handler.getCenterLinePath(target_lanes, s_start, s_end); @@ -437,6 +437,14 @@ ShiftLine getLaneChangingShiftLine( const Pose & lane_changing_start_pose = prepare_segment.points.back().point.pose; const Pose & lane_changing_end_pose = target_segment.points.front().point.pose; + return getLaneChangingShiftLine( + lane_changing_start_pose, lane_changing_end_pose, reference_path, shift_length); +} + +ShiftLine getLaneChangingShiftLine( + const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, + const PathWithLaneId & reference_path, const double shift_length) +{ ShiftLine shift_line; shift_line.end_shift_length = shift_length; shift_line.start = lane_changing_start_pose; diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index cbcec2e3095d3..c60bc53a1129e 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -226,8 +226,7 @@ void PlannerManager::generateCombinedDrivableArea( } else if (di.is_already_expanded) { // for single side shift utils::generateDrivableArea( - output.path, di.drivable_lanes, false, false, data->parameters.vehicle_length, data, - is_driving_forward); + output.path, di.drivable_lanes, false, false, false, data, is_driving_forward); } else { const auto shorten_lanes = utils::cutOverlappedLanes(output.path, di.drivable_lanes); @@ -239,7 +238,7 @@ void PlannerManager::generateCombinedDrivableArea( // for other modules where multiple modules may be launched utils::generateDrivableArea( output.path, expanded_lanes, di.enable_expanding_hatched_road_markings, - di.enable_expanding_intersection_areas, data->parameters.vehicle_length, data, + di.enable_expanding_intersection_areas, di.enable_expanding_freespace_areas, data, is_driving_forward); } @@ -630,32 +629,38 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrisKeepLast(); + const auto get_sorted_keep_last_modules = [this](const auto & modules) { + std::vector keep_last_modules; + + std::copy_if( + modules.begin(), modules.end(), std::back_inserter(keep_last_modules), + [this](const auto & m) { return getManager(m)->isKeepLast(); }); + + // sort by priority (low -> high) + std::sort( + keep_last_modules.begin(), keep_last_modules.end(), [this](const auto & a, const auto & b) { + return getManager(a)->getPriority() < getManager(b)->getPriority(); + }); + + return keep_last_modules; }; - move_to_end(approved_module_ptrs_, keep_last_module_cond); + + for (const auto & module : get_sorted_keep_last_modules(approved_module_ptrs_)) { + move_to_end(approved_module_ptrs_, module); + } } // lock approved modules besides last one @@ -768,6 +773,25 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrgetCurrentStatus() == ModuleStatus::SUCCESS; }; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp index 31dc117fbce35..28943b5724fe8 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp @@ -106,6 +106,7 @@ struct DrivableAreaInfo std::vector obstacles{}; // obstacles to extract from the drivable area bool enable_expanding_hatched_road_markings{false}; bool enable_expanding_intersection_areas{false}; + bool enable_expanding_freespace_areas{false}; // temporary only for pull over's freespace planning double drivable_margin{0.0}; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp index 9053da45708a0..9afa2608e1db6 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp @@ -19,6 +19,7 @@ #include #include +#include #include namespace behavior_path_planner::utils { @@ -40,8 +41,8 @@ std::vector getNonOverlappingExpandedLanes( void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const double vehicle_length, const std::shared_ptr planner_data, - const bool is_driving_forward = true); + const bool enable_expanding_freespace_areas, + const std::shared_ptr planner_data, const bool is_driving_forward = true); void generateDrivableArea( PathWithLaneId & path, const double vehicle_length, const double offset, @@ -62,6 +63,11 @@ std::vector expandLanelets( void extractObstaclesFromDrivableArea( PathWithLaneId & path, const std::vector & obstacles); +std::pair, bool> getBoundWithFreeSpaceAreas( + const std::vector & original_bound, + const std::vector & other_side_bound, + const std::shared_ptr planner_data, const bool is_left); + std::vector getBoundWithHatchedRoadMarkings( const std::vector & original_bound, const std::shared_ptr & route_handler); @@ -72,14 +78,22 @@ std::vector getBoundWithIntersectionAreas( const std::vector & drivable_lanes, const bool is_left); std::vector calcBound( - const std::shared_ptr route_handler, + const PathWithLaneId & path, const std::shared_ptr planner_data, + const std::vector & drivable_lanes, + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, + const bool enable_expanding_freespace_areas, const bool is_left, + const bool is_driving_forward = true); + +std::vector postProcess( + const std::vector & original_bound, const PathWithLaneId & path, + const std::shared_ptr planner_data, const std::vector & drivable_lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const bool is_left); + const bool is_left, const bool is_driving_forward = true); -void makeBoundLongitudinallyMonotonic( - PathWithLaneId & path, const std::shared_ptr & planner_data, - const bool is_bound_left); +std::vector makeBoundLongitudinallyMonotonic( + const std::vector & original_bound, const PathWithLaneId & path, + const std::shared_ptr & planner_data, const bool is_left); DrivableAreaInfo combineDrivableAreaInfo( const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2); diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 4e25e9257ddfd..18627e8396239 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -154,6 +154,7 @@ std::vector extractBoundFromPolygon( const bool clockwise) { std::vector ret{}; + for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) { ret.push_back(polygon[i]); @@ -765,54 +766,27 @@ std::vector getNonOverlappingExpandedLanes( void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const double vehicle_length, const std::shared_ptr planner_data, - const bool is_driving_forward) + const bool enable_expanding_freespace_areas, + const std::shared_ptr planner_data, const bool is_driving_forward) { - // extract data - const auto transformed_lanes = utils::transformToLanelets(lanes); - const auto current_pose = planner_data->self_odometry->pose.pose; - const auto route_handler = planner_data->route_handler; - constexpr double overlap_threshold = 0.01; - if (path.points.empty()) { return; } - auto addPoints = - [](const lanelet::ConstLineString3d & points, std::vector & bound) { - for (const auto & bound_p : points) { - const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p); - if (bound.empty()) { - bound.push_back(cp); - } else if (tier4_autoware_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { - bound.push_back(cp); - } - } - }; - - const auto has_overlap = - [&](const lanelet::ConstLanelet & lane, const lanelet::ConstLanelets & ignore_lanelets = {}) { - for (const auto & transformed_lane : transformed_lanes) { - if (checkHasSameLane(ignore_lanelets, transformed_lane)) { - continue; - } - if (boost::geometry::intersects( - lane.polygon2d().basicPolygon(), transformed_lane.polygon2d().basicPolygon())) { - return true; - } - } - return false; - }; + path.left_bound.clear(); + path.right_bound.clear(); // Insert Position - auto left_bound = calcBound( - route_handler, lanes, enable_expanding_hatched_road_markings, - enable_expanding_intersection_areas, true); - auto right_bound = calcBound( - route_handler, lanes, enable_expanding_hatched_road_markings, - enable_expanding_intersection_areas, false); - - if (left_bound.empty() || right_bound.empty()) { + path.left_bound = calcBound( + path, planner_data, lanes, enable_expanding_hatched_road_markings, + enable_expanding_intersection_areas, enable_expanding_freespace_areas, true, + is_driving_forward); + path.right_bound = calcBound( + path, planner_data, lanes, enable_expanding_hatched_road_markings, + enable_expanding_intersection_areas, enable_expanding_freespace_areas, false, + is_driving_forward); + + if (path.left_bound.empty() || path.right_bound.empty()) { auto clock{rclcpp::Clock{RCL_ROS_TIME}}; RCLCPP_ERROR_STREAM_THROTTLE( rclcpp::get_logger("behavior_path_planner").get_child("utils"), clock, 1000, @@ -820,135 +794,10 @@ void generateDrivableArea( return; } - // Insert points after goal - lanelet::ConstLanelet goal_lanelet; - if ( - route_handler->getGoalLanelet(&goal_lanelet) && - checkHasSameLane(transformed_lanes, goal_lanelet)) { - const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length); - const auto next_lanes_after_goal = route_handler->getNextLanelets(goal_lanelet); - const auto goal_left_lanelet = route_handler->getLeftLanelet(goal_lanelet); - const auto goal_right_lanelet = route_handler->getRightLanelet(goal_lanelet); - lanelet::ConstLanelets goal_lanelets = {goal_lanelet}; - if (goal_left_lanelet) { - goal_lanelets.push_back(*goal_left_lanelet); - } - if (goal_right_lanelet) { - goal_lanelets.push_back(*goal_right_lanelet); - } - - for (const auto & lane : lanes_after_goal) { - // If lane is already in the transformed lanes, ignore it - if (checkHasSameLane(transformed_lanes, lane)) { - continue; - } - // Check if overlapped - const bool is_overlapped = - (checkHasSameLane(next_lanes_after_goal, lane) ? has_overlap(lane, goal_lanelets) - : has_overlap(lane)); - if (is_overlapped) { - continue; - } - - addPoints(lane.leftBound3d(), left_bound); - addPoints(lane.rightBound3d(), right_bound); - } - } - - if (!is_driving_forward) { - std::reverse(left_bound.begin(), left_bound.end()); - std::reverse(right_bound.begin(), right_bound.end()); - } - - path.left_bound.clear(); - path.right_bound.clear(); - - const auto [left_start_idx, right_start_idx] = [&]() { - const size_t current_seg_idx = planner_data->findEgoSegmentIndex(path.points); - const auto cropped_path_points = motion_utils::cropPoints( - path.points, current_pose.position, current_seg_idx, - planner_data->parameters.forward_path_length, - planner_data->parameters.backward_path_length + planner_data->parameters.input_path_interval); - - constexpr double front_length = 0.5; - const auto front_pose = - cropped_path_points.empty() ? current_pose : cropped_path_points.front().point.pose; - const size_t front_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, front_pose, M_PI_2); - const size_t front_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, front_pose, M_PI_2); - const auto left_start_point = - calcLongitudinalOffsetStartPoint(left_bound, front_pose, front_left_start_idx, -front_length); - const auto right_start_point = calcLongitudinalOffsetStartPoint( - right_bound, front_pose, front_right_start_idx, -front_length); - const size_t left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, left_start_point); - const size_t right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, right_start_point); - - // Insert a start point - path.left_bound.push_back(left_start_point); - path.right_bound.push_back(right_start_point); - - return std::make_pair(left_start_idx, right_start_idx); - }(); - - // Get Closest segment for the goal point - const auto goal_pose = path.points.empty() ? current_pose : path.points.back().point.pose; - const size_t goal_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose, M_PI_2); - const size_t goal_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, goal_pose, M_PI_2); - const auto left_goal_point = - calcLongitudinalOffsetGoalPoint(left_bound, goal_pose, goal_left_start_idx, vehicle_length); - const auto right_goal_point = - calcLongitudinalOffsetGoalPoint(right_bound, goal_pose, goal_right_start_idx, vehicle_length); - const size_t left_goal_idx = std::max( - goal_left_start_idx, findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_point)); - const size_t right_goal_idx = std::max( - goal_right_start_idx, - findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_point)); - - // Insert middle points - for (size_t i = left_start_idx + 1; i <= left_goal_idx; ++i) { - const auto & next_point = left_bound.at(i); - const double dist = tier4_autoware_utils::calcDistance2d(path.left_bound.back(), next_point); - if (dist > overlap_threshold) { - path.left_bound.push_back(next_point); - } - } - for (size_t i = right_start_idx + 1; i <= right_goal_idx; ++i) { - const auto & next_point = right_bound.at(i); - const double dist = tier4_autoware_utils::calcDistance2d(path.right_bound.back(), next_point); - if (dist > overlap_threshold) { - path.right_bound.push_back(next_point); - } - } - - // Insert a goal point - if ( - tier4_autoware_utils::calcDistance2d(path.left_bound.back(), left_goal_point) > - overlap_threshold) { - path.left_bound.push_back(left_goal_point); - } - if ( - tier4_autoware_utils::calcDistance2d(path.right_bound.back(), right_goal_point) > - overlap_threshold) { - path.right_bound.push_back(right_goal_point); - } const auto & expansion_params = planner_data->drivable_area_expansion_parameters; if (expansion_params.enabled) { drivable_area_expansion::expand_drivable_area(path, planner_data); } - - // make bound longitudinally monotonic - // TODO(Murooka) Fix makeBoundLongitudinallyMonotonic - if ( - is_driving_forward && - (enable_expanding_hatched_road_markings || enable_expanding_intersection_areas)) { - makeBoundLongitudinallyMonotonic(path, planner_data, true); // for left bound - makeBoundLongitudinallyMonotonic(path, planner_data, false); // for right bound - } } void generateDrivableArea( @@ -1400,15 +1249,333 @@ std::vector getBoundWithIntersectionAreas( return expanded_bound; } +std::pair, bool> getBoundWithFreeSpaceAreas( + const std::vector & original_bound, + const std::vector & other_side_bound, + const std::shared_ptr planner_data, const bool is_left) +{ + using lanelet::utils::to2D; + using lanelet::utils::conversion::toGeomMsgPt; + using lanelet::utils::conversion::toLaneletPoint; + using tier4_autoware_utils::getPose; + using tier4_autoware_utils::pose2transform; + using tier4_autoware_utils::transformVector; + + const auto & route_handler = planner_data->route_handler; + const auto & ego_pose = planner_data->self_odometry->pose.pose; + + auto polygons = lanelet::utils::query::getAllParkingLots(route_handler->getLaneletMapPtr()); + if (polygons.empty()) { + return std::make_pair(original_bound, false); + } + + std::sort(polygons.begin(), polygons.end(), [&ego_pose](const auto & a, const auto & b) { + const double a_distance = boost::geometry::distance( + to2D(a).basicPolygon(), to2D(toLaneletPoint(ego_pose.position)).basicPoint()); + const double b_distance = boost::geometry::distance( + to2D(b).basicPolygon(), to2D(toLaneletPoint(ego_pose.position)).basicPoint()); + return a_distance < b_distance; + }); + + const auto & polygon = polygons.front(); + + const auto original_bound_itr = + std::find_if(original_bound.begin(), original_bound.end(), [&polygon](const auto & p) { + return std::any_of(polygon.begin(), polygon.end(), [&p](const auto & p_outer) { + return p.id() == p_outer.id(); + }); + }); + + const auto original_bound_rev_itr = + std::find_if(original_bound.rbegin(), original_bound.rend(), [&polygon](const auto & p) { + return std::any_of(polygon.begin(), polygon.end(), [&p](const auto & p_outer) { + return p.id() == p_outer.id(); + }); + }); + + const auto other_side_bound_itr = + std::find_if(other_side_bound.begin(), other_side_bound.end(), [&polygon](const auto & p) { + return std::any_of(polygon.begin(), polygon.end(), [&p](const auto & p_outer) { + return p.id() == p_outer.id(); + }); + }); + + if ( + original_bound_itr == original_bound.end() || other_side_bound_itr == other_side_bound.end()) { + return std::make_pair(original_bound, false); + } + + const auto footprint = planner_data->parameters.vehicle_info.createFootprint(); + const auto vehicle_polygon = transformVector(footprint, pose2transform(ego_pose)); + const auto is_driving_freespace = + !boost::geometry::disjoint(vehicle_polygon, to2D(polygon).basicPolygon()); + + const auto is_clockwise_polygon = boost::geometry::is_valid(to2D(polygon.basicPolygon())); + const auto is_clockwise_iteration = is_clockwise_polygon ? is_left : !is_left; + + const auto extract_bound_from_polygon = [&polygon, &is_clockwise_iteration]( + const auto & start_id, const auto & end_id) { + const auto start_point_itr = std::find_if( + polygon.begin(), polygon.end(), [&](const auto & p) { return p.id() == start_id; }); + + const auto end_point_itr = std::find_if( + polygon.begin(), polygon.end(), [&](const auto & p) { return p.id() == end_id; }); + + // extract line strings between start_idx and end_idx. + const size_t start_idx = std::distance(polygon.begin(), start_point_itr); + const size_t end_idx = std::distance(polygon.begin(), end_point_itr); + + return extractBoundFromPolygon(polygon, start_idx, end_idx, is_clockwise_iteration); + }; + + const auto get_bound_edge = [&ego_pose, &is_driving_freespace, &is_left]( + const auto & bound, const auto trim_behind_bound) { + if (!is_driving_freespace) { + return bound; + } + + const auto p_offset = tier4_autoware_utils::calcOffsetPose( + ego_pose, (trim_behind_bound ? -100.0 : 100.0), (is_left ? 0.1 : -0.1), 0.0); + + std::vector ret; + for (size_t i = 1; i < bound.size(); ++i) { + const auto intersect = tier4_autoware_utils::intersect( + ego_pose.position, p_offset.position, toGeomMsgPt(bound.at(i - 1)), + toGeomMsgPt(bound.at(i))); + + ret.push_back(bound.at(i - 1)); + + if (intersect.has_value()) { + ret.emplace_back( + lanelet::InvalId, intersect.value().x, intersect.value().y, intersect.value().z); + break; + } + } + + return ret; + }; + + std::vector expanded_bound; + + enum class RouteCase { + ROUTE_IS_PASS_THROUGH_FREESPACE = 0, + GOAL_POS_IS_IN_FREESPACE, + INIT_POS_IS_IN_FREESPACE, + OTHER, + }; + + const auto route_case = [&]() { + if (original_bound_itr->id() != original_bound_rev_itr->id()) { + return RouteCase::ROUTE_IS_PASS_THROUGH_FREESPACE; + } else if (boost::geometry::within( + to2D(original_bound.front().basicPoint()), to2D(polygon).basicPolygon())) { + return RouteCase::INIT_POS_IS_IN_FREESPACE; + } else if (boost::geometry::within( + to2D(original_bound.back().basicPoint()), to2D(polygon).basicPolygon())) { + return RouteCase::GOAL_POS_IS_IN_FREESPACE; + } + return RouteCase::OTHER; + }(); + + switch (route_case) { + case RouteCase::ROUTE_IS_PASS_THROUGH_FREESPACE: { + const auto polygon_bound = + extract_bound_from_polygon(original_bound_itr->id(), original_bound_rev_itr->id()); + + expanded_bound.insert(expanded_bound.end(), original_bound.begin(), original_bound_itr); + expanded_bound.insert(expanded_bound.end(), polygon_bound.begin(), polygon_bound.end()); + expanded_bound.insert( + expanded_bound.end(), std::next(original_bound_rev_itr).base(), original_bound.end()); + break; + } + case RouteCase::INIT_POS_IS_IN_FREESPACE: { + auto polygon_bound = + extract_bound_from_polygon(other_side_bound_itr->id(), original_bound_itr->id()); + std::reverse(polygon_bound.begin(), polygon_bound.end()); + auto bound_edge = get_bound_edge(polygon_bound, true); + std::reverse(bound_edge.begin(), bound_edge.end()); + + expanded_bound.insert(expanded_bound.end(), bound_edge.begin(), bound_edge.end()); + expanded_bound.insert(expanded_bound.end(), original_bound_itr, original_bound.end()); + break; + } + case RouteCase::GOAL_POS_IS_IN_FREESPACE: { + const auto polygon_bound = + extract_bound_from_polygon(original_bound_itr->id(), other_side_bound_itr->id()); + const auto bound_edge = get_bound_edge(polygon_bound, false); + + expanded_bound.insert(expanded_bound.end(), original_bound.begin(), original_bound_itr); + expanded_bound.insert(expanded_bound.end(), bound_edge.begin(), bound_edge.end()); + break; + } + case RouteCase::OTHER: { + expanded_bound = original_bound; + break; + } + default: + throw std::domain_error("invalid case."); + } + + const auto goal_is_in_freespace = boost::geometry::within( + to2D(toLaneletPoint(route_handler->getGoalPose().position).basicPoint()), + to2D(polygon).basicPolygon()); + + return std::make_pair(expanded_bound, is_driving_freespace || goal_is_in_freespace); +} + +std::vector postProcess( + const std::vector & original_bound, const PathWithLaneId & path, + const std::shared_ptr planner_data, + const std::vector & drivable_lanes, + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, + const bool is_left, const bool is_driving_forward) +{ + const auto lanelets = utils::transformToLanelets(drivable_lanes); + const auto & current_pose = planner_data->self_odometry->pose.pose; + const auto & route_handler = planner_data->route_handler; + const auto & vehicle_length = planner_data->parameters.vehicle_length; + constexpr double overlap_threshold = 0.01; + + const auto addPoints = + [](const lanelet::ConstLineString3d & points, std::vector & bound) { + for (const auto & bound_p : points) { + const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p); + if (bound.empty()) { + bound.push_back(cp); + } else if (tier4_autoware_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { + bound.push_back(cp); + } + } + }; + + const auto has_overlap = + [&](const lanelet::ConstLanelet & lane, const lanelet::ConstLanelets & ignore_lanelets = {}) { + for (const auto & transformed_lane : lanelets) { + if (checkHasSameLane(ignore_lanelets, transformed_lane)) { + continue; + } + if (boost::geometry::intersects( + lane.polygon2d().basicPolygon(), transformed_lane.polygon2d().basicPolygon())) { + return true; + } + } + return false; + }; + + std::vector processed_bound; + std::vector tmp_bound = original_bound; + + // Insert points after goal + lanelet::ConstLanelet goal_lanelet; + if (route_handler->getGoalLanelet(&goal_lanelet) && checkHasSameLane(lanelets, goal_lanelet)) { + const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length); + const auto next_lanes_after_goal = route_handler->getNextLanelets(goal_lanelet); + const auto goal_left_lanelet = route_handler->getLeftLanelet(goal_lanelet); + const auto goal_right_lanelet = route_handler->getRightLanelet(goal_lanelet); + lanelet::ConstLanelets goal_lanelets = {goal_lanelet}; + if (goal_left_lanelet.has_value()) { + goal_lanelets.push_back(goal_left_lanelet.value()); + } + if (goal_right_lanelet.has_value()) { + goal_lanelets.push_back(goal_right_lanelet.value()); + } + + for (const auto & lane : lanes_after_goal) { + // If lane is already in the transformed lanes, ignore it + if (checkHasSameLane(lanelets, lane)) { + continue; + } + // Check if overlapped + const bool is_overlapped = + (checkHasSameLane(next_lanes_after_goal, lane) ? has_overlap(lane, goal_lanelets) + : has_overlap(lane)); + if (is_overlapped) { + continue; + } + + if (is_left) { + addPoints(lane.leftBound3d(), tmp_bound); + } else { + addPoints(lane.rightBound3d(), tmp_bound); + } + } + } + + if (!is_driving_forward) { + std::reverse(tmp_bound.begin(), tmp_bound.end()); + } + + const auto start_idx = [&]() { + const size_t current_seg_idx = planner_data->findEgoSegmentIndex(path.points); + const auto cropped_path_points = motion_utils::cropPoints( + path.points, current_pose.position, current_seg_idx, + planner_data->parameters.forward_path_length, + planner_data->parameters.backward_path_length + planner_data->parameters.input_path_interval); + + constexpr double front_length = 0.5; + const auto front_pose = + cropped_path_points.empty() ? current_pose : cropped_path_points.front().point.pose; + const size_t front_start_idx = + findNearestSegmentIndexFromLateralDistance(tmp_bound, front_pose, M_PI_2); + const auto start_point = + calcLongitudinalOffsetStartPoint(tmp_bound, front_pose, front_start_idx, -front_length); + + // Insert a start point + processed_bound.push_back(start_point); + + return findNearestSegmentIndexFromLateralDistance(tmp_bound, start_point); + }(); + + // Get Closest segment for the goal point + const auto [goal_idx, goal_point] = [&]() { + const auto goal_pose = path.points.empty() ? current_pose : path.points.back().point.pose; + const size_t goal_start_idx = + findNearestSegmentIndexFromLateralDistance(tmp_bound, goal_pose, M_PI_2); + const auto goal_point = + calcLongitudinalOffsetGoalPoint(tmp_bound, goal_pose, goal_start_idx, vehicle_length); + const size_t goal_idx = + std::max(goal_start_idx, findNearestSegmentIndexFromLateralDistance(tmp_bound, goal_point)); + + return std::make_pair(goal_idx, goal_point); + }(); + + // Insert middle points + for (size_t i = start_idx + 1; i <= goal_idx; ++i) { + const auto & next_point = tmp_bound.at(i); + const double dist = tier4_autoware_utils::calcDistance2d(processed_bound.back(), next_point); + if (dist > overlap_threshold) { + processed_bound.push_back(next_point); + } + } + + // Insert a goal point + if ( + tier4_autoware_utils::calcDistance2d(processed_bound.back(), goal_point) > overlap_threshold) { + processed_bound.push_back(goal_point); + } + + const bool skip_monotonic_process = + !is_driving_forward || + !(enable_expanding_hatched_road_markings || enable_expanding_intersection_areas); + + return skip_monotonic_process + ? processed_bound + : makeBoundLongitudinallyMonotonic(processed_bound, path, planner_data, is_left); +} + // calculate bounds from drivable lanes and hatched road markings std::vector calcBound( - const std::shared_ptr route_handler, + const PathWithLaneId & path, const std::shared_ptr planner_data, const std::vector & drivable_lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const bool is_left) + const bool enable_expanding_freespace_areas, const bool is_left, const bool is_driving_forward) { + using motion_utils::removeOverlapPoints; + + const auto & route_handler = planner_data->route_handler; + // a function to convert drivable lanes to points without duplicated points - const auto convert_to_points = [&](const std::vector & drivable_lanes) { + const auto convert_to_points = [](const auto & drivable_lanes, const auto is_left) { constexpr double overlap_threshold = 0.01; std::vector points; @@ -1435,20 +1602,35 @@ std::vector calcBound( }; // Step1. create drivable bound from drivable lanes. - std::vector bound_points = convert_to_points(drivable_lanes); + auto [bound_points, skip_post_process] = [&]() { + if (!enable_expanding_freespace_areas) { + return std::make_pair(convert_to_points(drivable_lanes, is_left), false); + } + return getBoundWithFreeSpaceAreas( + convert_to_points(drivable_lanes, is_left), convert_to_points(drivable_lanes, !is_left), + planner_data, is_left); + }(); + + const auto post_process = [&](const auto & bound, const auto skip) { + return skip + ? bound + : postProcess( + bound, path, planner_data, drivable_lanes, enable_expanding_hatched_road_markings, + enable_expanding_intersection_areas, is_left, is_driving_forward); + }; // Step2. if there is no drivable area defined by polygon, return original drivable bound. if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas) { - return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); + return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process); } - // Step3. if there are hatched road markings, expand drivable bound with the polygon. + // Step3.if there are hatched road markings, expand drivable bound with the polygon. if (enable_expanding_hatched_road_markings) { bound_points = getBoundWithHatchedRoadMarkings(bound_points, route_handler); } if (!enable_expanding_intersection_areas) { - return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); + return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process); } // Step4. if there are intersection areas, expand drivable bound with the polygon. @@ -1457,12 +1639,12 @@ std::vector calcBound( getBoundWithIntersectionAreas(bound_points, route_handler, drivable_lanes, is_left); } - return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); + return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process); } -void makeBoundLongitudinallyMonotonic( - PathWithLaneId & path, const std::shared_ptr & planner_data, - const bool is_bound_left) +std::vector makeBoundLongitudinallyMonotonic( + const std::vector & original_bound, const PathWithLaneId & path, + const std::shared_ptr & planner_data, const bool is_left) { using motion_utils::findNearestSegmentIndex; using tier4_autoware_utils::calcAzimuthAngle; @@ -1508,7 +1690,7 @@ void makeBoundLongitudinallyMonotonic( const auto get_bound_with_pose = [&](const auto & bound_with_pose, const auto & path_points) { auto ret = bound_with_pose; - const double offset = is_bound_left ? 100.0 : -100.0; + const double offset = is_left ? 100.0 : -100.0; size_t start_bound_idx = 0; @@ -1588,14 +1770,14 @@ void makeBoundLongitudinallyMonotonic( // NOTE: is_bound_left is used instead of is_points_left since orientation of path point is // opposite. - const double lat_offset = is_bound_left ? 100.0 : -100.0; + const double lat_offset = is_left ? 100.0 : -100.0; const auto p_bound_1 = getPose(bound_with_pose.at(bound_idx)); const auto p_bound_2 = getPose(bound_with_pose.at(bound_idx + 1)); const auto p_bound_offset = calcOffsetPose(p_bound_1, 0.0, lat_offset, 0.0); - if (!is_non_monotonic(p_bound_1, p_bound_2.position, is_bound_left, is_reverse)) { + if (!is_non_monotonic(p_bound_1, p_bound_2.position, is_left, is_reverse)) { bound_idx++; continue; } @@ -1681,18 +1863,16 @@ void makeBoundLongitudinallyMonotonic( return ret; }; - const auto original_bound = is_bound_left ? path.left_bound : path.right_bound; - if (path.points.empty()) { - return; + return original_bound; } - if (original_bound.empty()) { - return; + if (original_bound.size() < 2) { + return original_bound; } if (path.points.front().lane_ids.empty()) { - return; + return original_bound; } // step.1 create bound with pose vector. @@ -1734,14 +1914,14 @@ void makeBoundLongitudinallyMonotonic( p.forward_path_length, p); if (centerline_path.points.size() < 2) { - return; + return original_bound; } const auto ego_idx = planner_data->findEgoIndex(centerline_path.points); const auto end_idx = findNearestSegmentIndex(centerline_path.points, original_bound.back()); if (ego_idx >= end_idx) { - return; + return original_bound; } clipped_points.insert( @@ -1750,7 +1930,7 @@ void makeBoundLongitudinallyMonotonic( } if (clipped_points.empty()) { - return; + return original_bound; } // step.3 update bound pose by reference path pose. @@ -1771,11 +1951,7 @@ void makeBoundLongitudinallyMonotonic( auto full_monotonic_bound = remove_orientation(full_monotonic_bound_with_pose); // step.7 remove sharp bound points. - if (is_bound_left) { - path.left_bound = remove_sharp_points(full_monotonic_bound); - } else { - path.right_bound = remove_sharp_points(full_monotonic_bound); - } + return remove_sharp_points(full_monotonic_bound); } std::vector combineDrivableLanes( @@ -1785,7 +1961,9 @@ std::vector combineDrivableLanes( const auto has_same_lane = [](const lanelet::ConstLanelet & target_lane, const lanelet::ConstLanelets & lanes) { return std::find_if(lanes.begin(), lanes.end(), [&](const auto & ll) { - return ll.id() == target_lane.id(); + return ll.id() == target_lane.id() && + ll.rightBound3d().id() == target_lane.rightBound3d().id() && + ll.leftBound3d().id() == target_lane.leftBound3d().id(); }) != lanes.end(); }; @@ -1884,6 +2062,11 @@ DrivableAreaInfo combineDrivableAreaInfo( drivable_area_info1.enable_expanding_intersection_areas || drivable_area_info2.enable_expanding_intersection_areas; + // enable expanding freespace areas + combined_drivable_area_info.enable_expanding_freespace_areas = + drivable_area_info1.enable_expanding_freespace_areas || + drivable_area_info2.enable_expanding_freespace_areas; + // drivable margin combined_drivable_area_info.drivable_margin = std::max(drivable_area_info1.drivable_margin, drivable_area_info2.drivable_margin); diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index d174b54b9ccbe..9e5a05a2d60cd 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -5,15 +5,14 @@ th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 1.0 - collision_check_margins: [2.0, 1.5, 1.0] - collision_check_distance_from_end: 1.0 + collision_check_margins: [2.0, 1.0, 0.5, 0.1] collision_check_margin_from_front_object: 5.0 th_moving_object_velocity: 1.0 th_distance_to_middle_of_the_road: 0.5 center_line_path_interval: 1.0 # shift pull out enable_shift_pull_out: true - check_shift_path_lane_departure: false + shift_collision_check_distance_from_end: -10.0 minimum_shift_pull_out_distance: 0.0 deceleration_interval: 15.0 lateral_jerk: 0.5 @@ -23,6 +22,7 @@ maximum_curvature: 0.07 # geometric pull out enable_geometric_pull_out: true + geometric_collision_check_distance_from_end: 0.0 divide_pull_out_path: true geometric_pull_out_velocity: 1.0 arc_path_interval: 1.0 @@ -32,7 +32,7 @@ # search start pose backward enable_back: true search_priority: "efficient_path" # "efficient_path" or "short_back_distance" - max_back_distance: 30.0 + max_back_distance: 20.0 backward_search_resolution: 2.0 backward_path_update_duration: 3.0 ignore_distance_from_lane_end: 15.0 diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 8f8d2baf9c85e..f42aef4075777 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -63,7 +63,6 @@ struct StartPlannerParameters double intersection_search_length{0.0}; double length_ratio_for_turn_signal_deactivation_near_intersection{0.0}; std::vector collision_check_margins{}; - double collision_check_distance_from_end{0.0}; double collision_check_margin_from_front_object{0.0}; double th_moving_object_velocity{0.0}; double center_line_path_interval{0.0}; @@ -71,6 +70,7 @@ struct StartPlannerParameters // shift pull out bool enable_shift_pull_out{false}; bool check_shift_path_lane_departure{false}; + double shift_collision_check_distance_from_end{0.0}; double minimum_shift_pull_out_distance{0.0}; int lateral_acceleration_sampling_num{0}; double lateral_jerk{0.0}; @@ -80,6 +80,7 @@ struct StartPlannerParameters double deceleration_interval{0.0}; // geometric pull out bool enable_geometric_pull_out{false}; + double geometric_collision_check_distance_from_end; bool divide_pull_out_path{false}; ParallelParkingParameters parallel_parking_parameters{}; // search start pose backward diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index cffce8218c554..90ce99692e57a 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -164,6 +164,7 @@ class StartPlannerModule : public SceneModuleInterface bool isModuleRunning() const; bool isCurrentPoseOnMiddleOfTheRoad() const; + bool isOverlapWithCenterLane() const; bool isCloseToOriginalStartPose() const; bool hasArrivedAtGoal() const; bool isMoving() const; @@ -174,7 +175,8 @@ class StartPlannerModule : public SceneModuleInterface const Pose & start_pose_candidate, const std::shared_ptr & planner, const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin); - PathWithLaneId extractCollisionCheckSection(const PullOutPath & path); + PathWithLaneId extractCollisionCheckSection( + const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type); void updateStatusWithCurrentPath( const behavior_path_planner::PullOutPath & path, const Pose & start_pose, const behavior_path_planner::PlannerType & planner_type); diff --git a/planning/behavior_path_start_planner_module/package.xml b/planning/behavior_path_start_planner_module/package.xml index cbfdba0d07a57..f897188d60444 100644 --- a/planning/behavior_path_start_planner_module/package.xml +++ b/planning/behavior_path_start_planner_module/package.xml @@ -11,6 +11,8 @@ Tomoya Kimura Shumpei Wakabayashi Tomohito Ando + Mamoru Sobue + Daniel Sanchez Apache License 2.0 diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 8cc0b34082e44..7fb52d59c418b 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -45,8 +45,6 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); p.collision_check_margins = node->declare_parameter>(ns + "collision_check_margins"); - p.collision_check_distance_from_end = - node->declare_parameter(ns + "collision_check_distance_from_end"); p.collision_check_margin_from_front_object = node->declare_parameter(ns + "collision_check_margin_from_front_object"); p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); @@ -55,6 +53,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); p.check_shift_path_lane_departure = node->declare_parameter(ns + "check_shift_path_lane_departure"); + p.shift_collision_check_distance_from_end = + node->declare_parameter(ns + "shift_collision_check_distance_from_end"); p.minimum_shift_pull_out_distance = node->declare_parameter(ns + "minimum_shift_pull_out_distance"); p.lateral_acceleration_sampling_num = @@ -66,6 +66,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.deceleration_interval = node->declare_parameter(ns + "deceleration_interval"); // geometric pull out p.enable_geometric_pull_out = node->declare_parameter(ns + "enable_geometric_pull_out"); + p.geometric_collision_check_distance_from_end = + node->declare_parameter(ns + "geometric_collision_check_distance_from_end"); p.divide_pull_out_path = node->declare_parameter(ns + "divide_pull_out_path"); p.parallel_parking_parameters.pull_out_velocity = node->declare_parameter(ns + "geometric_pull_out_velocity"); diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 8937c8a837694..f5f0e514ce8bc 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -21,6 +21,7 @@ #include "behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" +#include #include #include #include @@ -211,7 +212,8 @@ bool StartPlannerModule::receivedNewRoute() const bool StartPlannerModule::requiresDynamicObjectsCollisionDetection() const { - return parameters_->safety_check_params.enable_safety_check && status_.driving_forward; + return parameters_->safety_check_params.enable_safety_check && status_.driving_forward && + !isOverlapWithCenterLane(); } bool StartPlannerModule::noMovingObjectsAround() const @@ -278,6 +280,37 @@ bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const return std::abs(lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road; } +bool StartPlannerModule::isOverlapWithCenterLane() const +{ + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + const auto current_lanes = utils::getCurrentLanes(planner_data_); + const auto local_vehicle_footprint = vehicle_info_.createFootprint(); + const auto vehicle_footprint = + transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(current_pose)); + for (const auto & current_lane : current_lanes) { + std::vector centerline_points; + for (const auto & point : current_lane.centerline()) { + geometry_msgs::msg::Point center_point = lanelet::utils::conversion::toGeomMsgPt(point); + centerline_points.push_back(center_point); + } + + for (size_t i = 0; i < centerline_points.size() - 1; ++i) { + const auto & p1 = centerline_points.at(i); + const auto & p2 = centerline_points.at(i + 1); + for (size_t j = 0; j < vehicle_footprint.size() - 1; ++j) { + const auto p3 = tier4_autoware_utils::toMsg(vehicle_footprint[j].to_3d()); + const auto p4 = tier4_autoware_utils::toMsg(vehicle_footprint[j + 1].to_3d()); + const auto intersection = tier4_autoware_utils::intersect(p1, p2, p3, p4); + + if (intersection.has_value()) { + return true; + } + } + } + } + return false; +} + bool StartPlannerModule::isCloseToOriginalStartPose() const { const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); @@ -657,8 +690,8 @@ bool StartPlannerModule::findPullOutPath( // check collision if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint, extractCollisionCheckSection(*pull_out_path), pull_out_lane_stop_objects, - collision_check_margin)) { + vehicle_footprint, extractCollisionCheckSection(*pull_out_path, planner->getPlannerType()), + pull_out_lane_stop_objects, collision_check_margin)) { return false; } @@ -672,8 +705,17 @@ bool StartPlannerModule::findPullOutPath( return true; } -PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPath & path) +PathWithLaneId StartPlannerModule::extractCollisionCheckSection( + const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type) { + const std::map collision_check_distances = { + {behavior_path_planner::PlannerType::SHIFT, + parameters_->shift_collision_check_distance_from_end}, + {behavior_path_planner::PlannerType::GEOMETRIC, + parameters_->geometric_collision_check_distance_from_end}}; + + const double collision_check_distance_from_end = collision_check_distances.at(planner_type); + PathWithLaneId combined_path; for (const auto & partial_path : path.partial_paths) { combined_path.points.insert( @@ -682,7 +724,7 @@ PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPat // calculate collision check end idx const size_t collision_check_end_idx = std::invoke([&]() { const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( - combined_path.points, path.end_pose.position, parameters_->collision_check_distance_from_end); + combined_path.points, path.end_pose.position, collision_check_distance_from_end); if (collision_check_end_pose) { return motion_utils::findNearestIndex( @@ -926,8 +968,8 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, 0, std::numeric_limits::max()); - // Set the maximum backward distance less than the distance from the vehicle's base_link to the - // lane's rearmost point to prevent lane departure. + // Set the maximum backward distance less than the distance from the vehicle's base_link to + // the lane's rearmost point to prevent lane departure. const double current_arc_length = lanelet::utils::getArcCoordinates(pull_out_lanes, start_pose).length; const double allowed_backward_distance = std::clamp( @@ -1215,8 +1257,8 @@ bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const { const auto & rh = planner_data_->route_handler; - // Check if the goal and ego are in the same route segment. If not, this is out of scope of this - // function. Return false. + // Check if the goal and ego are in the same route segment. If not, this is out of scope of + // this function. Return false. lanelet::ConstLanelet ego_lanelet; rh->getClosestLaneletWithinRoute(getEgoPose(), &ego_lanelet); const auto is_ego_in_goal_route_section = rh->isInGoalRouteSection(ego_lanelet); @@ -1403,9 +1445,14 @@ void StartPlannerModule::setDebugData() // visualize collision_check_end_pose and footprint { const auto local_footprint = vehicle_info_.createFootprint(); + std::map collision_check_distances = { + {PlannerType::SHIFT, parameters_->shift_collision_check_distance_from_end}, + {PlannerType::GEOMETRIC, parameters_->geometric_collision_check_distance_from_end}}; + + double collision_check_distance_from_end = collision_check_distances[status_.planner_type]; const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( getFullPath().points, status_.pull_out_path.end_pose.position, - parameters_->collision_check_distance_from_end); + collision_check_distance_from_end); if (collision_check_end_pose) { add(createPoseMarkerArray( *collision_check_end_pose, "static_collision_check_end_pose", 0, 1.0, 0.0, 0.0)); diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index efbff7e2af51d..54314e50308ff 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -348,7 +348,7 @@ bool BlindSpotModule::checkObstacleInBlindSpot( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) const + const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) { /* get detection area */ if (turn_direction_ == TurnDirection::INVALID) { @@ -409,6 +409,8 @@ bool BlindSpotModule::checkObstacleInBlindSpot( exist_in_right_conflict_area || exist_in_opposite_conflict_area; if (exist_in_detection_area && exist_in_conflict_area) { debug_data_.conflicting_targets.objects.push_back(object); + setObjectsOfInterestData( + object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED); return true; } } diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index 6f8450568939c..2ca2fe7950989 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -113,7 +113,7 @@ class BlindSpotModule : public SceneModuleInterface lanelet::routing::RoutingGraphPtr routing_graph_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) const; + const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose); /** * @brief Create half lanelet diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_intersection_module/CMakeLists.txt index 4c8fe5c6fa0f5..07847a08c1209 100644 --- a/planning/behavior_velocity_intersection_module/CMakeLists.txt +++ b/planning/behavior_velocity_intersection_module/CMakeLists.txt @@ -12,6 +12,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/util.cpp src/scene_intersection.cpp src/intersection_lanelets.cpp + src/object_manager.cpp + src/decision_result.cpp src/scene_intersection_prepare_data.cpp src/scene_intersection_stuck.cpp src/scene_intersection_occlusion.cpp diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 108e021240851..c7f6f62d575a0 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -59,6 +59,8 @@ object_expected_deceleration: 2.0 ignore_on_red_traffic_light: object_margin_to_path: 2.0 + avoid_collision_by_acceleration: + object_time_margin_to_collision_point: 4.0 occlusion: enable: false diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index 0c9b3407d0f38..0bed7d32f412f 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -22,10 +22,12 @@ autoware_auto_planning_msgs autoware_perception_msgs behavior_velocity_planner_common + fmt geometry_msgs interpolation lanelet2_extension libopencv-dev + magic_enum motion_utils nav_msgs pluginlib @@ -33,7 +35,6 @@ route_handler rtc_interface tf2_geometry_msgs - tier4_api_msgs tier4_autoware_utils tier4_planning_msgs vehicle_info_util diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 350f6d774f7cf..978855b36c5f6 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -31,8 +31,6 @@ #include #include -namespace behavior_velocity_planner -{ namespace { using tier4_autoware_utils::appendMarkerArray; @@ -40,14 +38,14 @@ using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerOrientation; using tier4_autoware_utils::createMarkerScale; -static visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( +visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( const std::vector & polygons, const std::string & ns, const int64_t lane_id, const double r, const double g, const double b) { visualization_msgs::msg::MarkerArray msg; int32_t i = 0; - int32_t uid = planning_utils::bitShift(lane_id); + int32_t uid = behavior_velocity_planner::planning_utils::bitShift(lane_id); for (const auto & polygon : polygons) { visualization_msgs::msg::Marker marker{}; marker.header.frame_id = "map"; @@ -158,8 +156,59 @@ visualization_msgs::msg::MarkerArray createLineMarkerArray( return marker_point; } +constexpr std::tuple white() +{ + constexpr uint64_t code = 0xfdfdfd; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple green() +{ + constexpr uint64_t code = 0x5fa641; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple yellow() +{ + constexpr uint64_t code = 0xebce2b; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple red() +{ + constexpr uint64_t code = 0xba1c30; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple light_blue() +{ + constexpr uint64_t code = 0x96cde6; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} } // namespace +namespace behavior_velocity_planner +{ +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerOrientation; +using tier4_autoware_utils::createMarkerScale; + visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray() { visualization_msgs::msg::MarkerArray debug_marker_array; @@ -168,14 +217,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.attention_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( debug_data_.attention_area.value(), "attention_area", lane_id_, 0.0, 1.0, 0.0), &debug_marker_array); } if (debug_data_.occlusion_attention_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( debug_data_.occlusion_attention_area.value(), "occlusion_attention_area", lane_id_, 0.917, 0.568, 0.596), &debug_marker_array); @@ -183,14 +232,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.adjacent_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( debug_data_.adjacent_area.value(), "adjacent_area", lane_id_, 0.913, 0.639, 0.149), &debug_marker_array); } if (debug_data_.first_attention_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( {debug_data_.first_attention_area.value()}, "first_attention_area", lane_id_, 1, 0.647, 0.0), &debug_marker_array, now); @@ -198,7 +247,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.second_attention_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( {debug_data_.second_attention_area.value()}, "second_attention_area", lane_id_, 1, 0.647, 0.0), &debug_marker_array, now); @@ -214,7 +263,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.yield_stuck_detect_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( debug_data_.yield_stuck_detect_area.value(), "yield_stuck_detect_area", lane_id_, 0.6588235, 0.34509, 0.6588235), &debug_marker_array); @@ -222,7 +271,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.ego_lane) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( {debug_data_.ego_lane.value()}, "ego_lane", lane_id_, 1, 0.647, 0.0), &debug_marker_array, now); } @@ -235,59 +284,58 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } - size_t i{0}; - for (const auto & p : debug_data_.candidate_collision_object_polygons) { - appendMarkerArray( - debug::createPolygonMarkerArray( - p, "candidate_collision_object_polygons", lane_id_ + i++, now, 0.3, 0.0, 0.0, 0.0, 0.5, - 0.5), - &debug_marker_array, now); - } - + static constexpr auto white = ::white(); + static constexpr auto green = ::green(); + static constexpr auto yellow = ::yellow(); + static constexpr auto red = ::red(); + static constexpr auto light_blue = ::light_blue(); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.conflicting_targets, "conflicting_targets", module_id_, now, 0.99, 0.4, 0.0), + debug_data_.safe_under_traffic_control_targets, "safe_under_traffic_control_targets", + module_id_, now, std::get<0>(light_blue), std::get<1>(light_blue), std::get<2>(light_blue)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.amber_ignore_targets, "amber_ignore_targets", module_id_, now, 0.0, 1.0, 0.0), + debug_data_.unsafe_targets, "unsafe_targets", module_id_, now, std::get<0>(green), + std::get<1>(green), std::get<2>(green)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.red_overshoot_ignore_targets, "red_overshoot_ignore_targets", module_id_, now, - 0.0, 1.0, 0.0), + debug_data_.misjudge_targets, "misjudge_targets", module_id_, now, std::get<0>(yellow), + std::get<1>(yellow), std::get<2>(yellow)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2), + debug_data_.too_late_detect_targets, "too_late_detect_targets", module_id_, now, + std::get<0>(red), std::get<1>(red), std::get<2>(red)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.yield_stuck_targets, "stuck_targets", module_id_, now, 0.4, 0.99, 0.2), + debug_data_.parked_targets, "parked_targets", module_id_, now, std::get<0>(white), + std::get<1>(white), std::get<2>(white)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.blocking_attention_objects, "blocking_attention_objects", module_id_, now, 0.99, - 0.99, 0.6), + debug_data_.stuck_targets, "stuck_targets", module_id_, now, std::get<0>(white), + std::get<1>(white), std::get<2>(white)), &debug_marker_array, now); - /* appendMarkerArray( - createPoseMarkerArray( - debug_data_.predicted_obj_pose, "predicted_obj_pose", module_id_, 0.7, 0.85, 0.9), + debug::createObjectsMarkerArray( + debug_data_.yield_stuck_targets, "yield_stuck_targets", module_id_, now, std::get<0>(white), + std::get<1>(white), std::get<2>(white)), &debug_marker_array, now); - */ if (debug_data_.first_pass_judge_wall_pose) { const double r = debug_data_.passed_first_pass_judge ? 1.0 : 0.0; const double g = debug_data_.passed_first_pass_judge ? 0.0 : 1.0; appendMarkerArray( - createPoseMarkerArray( + ::createPoseMarkerArray( debug_data_.first_pass_judge_wall_pose.value(), "first_pass_judge_wall_pose", module_id_, r, g, 0.0), &debug_marker_array, now); @@ -297,7 +345,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( const double r = debug_data_.passed_second_pass_judge ? 1.0 : 0.0; const double g = debug_data_.passed_second_pass_judge ? 0.0 : 1.0; appendMarkerArray( - createPoseMarkerArray( + ::createPoseMarkerArray( debug_data_.second_pass_judge_wall_pose.value(), "second_pass_judge_wall_pose", module_id_, r, g, 0.0), &debug_marker_array, now); @@ -314,7 +362,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.nearest_occlusion_projection) { const auto [point_start, point_end] = debug_data_.nearest_occlusion_projection.value(); appendMarkerArray( - createLineMarkerArray( + ::createLineMarkerArray( point_start, point_end, "nearest_occlusion_projection", lane_id_, 0.5, 0.5, 0.0), &debug_marker_array, now); } @@ -369,11 +417,11 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMark const auto state = state_machine_.getState(); - int32_t uid = planning_utils::bitShift(module_id_); + int32_t uid = behavior_velocity_planner::planning_utils::bitShift(module_id_); const auto now = this->clock_->now(); if (state == StateMachine::State::STOP) { appendMarkerArray( - createPoseMarkerArray(debug_data_.stop_point_pose, "stop_point_pose", uid, 1.0, 0.0, 0.0), + ::createPoseMarkerArray(debug_data_.stop_point_pose, "stop_point_pose", uid, 1.0, 0.0, 0.0), &debug_marker_array, now); } diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.cpp b/planning/behavior_velocity_intersection_module/src/decision_result.cpp new file mode 100644 index 0000000000000..aefd59a72f9b4 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/decision_result.cpp @@ -0,0 +1,65 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "decision_result.hpp" + +namespace behavior_velocity_planner::intersection +{ +std::string formatDecisionResult(const DecisionResult & decision_result) +{ + if (std::holds_alternative(decision_result)) { + const auto & state = std::get(decision_result); + return "InternalError because " + state.error; + } + if (std::holds_alternative(decision_result)) { + const auto & state = std::get(decision_result); + return "OverPassJudge:\nsafety_report:" + state.safety_report + "\nevasive_report:\n" + + state.evasive_report; + } + if (std::holds_alternative(decision_result)) { + return "StuckStop"; + } + if (std::holds_alternative(decision_result)) { + const auto & state = std::get(decision_result); + return "YieldStuckStop:\nsafety_report:" + state.safety_report; + } + if (std::holds_alternative(decision_result)) { + const auto & state = std::get(decision_result); + return "NonOccludedCollisionStop\nsafety_report:" + state.safety_report; + } + if (std::holds_alternative(decision_result)) { + return "FirstWaitBeforeOcclusion"; + } + if (std::holds_alternative(decision_result)) { + return "PeekingTowardOcclusion"; + } + if (std::holds_alternative(decision_result)) { + const auto & state = std::get(decision_result); + return "OccludedCollisionStop\nsafety_report:" + state.safety_report; + } + if (std::holds_alternative(decision_result)) { + const auto & state = std::get(decision_result); + return "OccludedAbsenceTrafficLight\nsafety_report:" + state.safety_report; + } + if (std::holds_alternative(decision_result)) { + return "Safe"; + } + if (std::holds_alternative(decision_result)) { + const auto & state = std::get(decision_result); + return "FullyPrioritized\nsafety_report:" + state.safety_report; + } + return ""; +} + +} // namespace behavior_velocity_planner::intersection diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.hpp b/planning/behavior_velocity_intersection_module/src/decision_result.hpp index 48b0ecf1349a5..da71168c2c4ca 100644 --- a/planning/behavior_velocity_intersection_module/src/decision_result.hpp +++ b/planning/behavior_velocity_intersection_module/src/decision_result.hpp @@ -23,16 +23,23 @@ namespace behavior_velocity_planner::intersection { /** - * @struct - * @brief Internal error or ego already passed pass_judge_line + * @brief internal error */ -struct Indecisive +struct InternalError { std::string error; }; /** - * @struct + * @brief + */ +struct OverPassJudge +{ + std::string safety_report; + std::string evasive_report; +}; + +/** * @brief detected stuck vehicle */ struct StuckStop @@ -43,17 +50,16 @@ struct StuckStop }; /** - * @struct * @brief yielded by vehicle on the attention area */ struct YieldStuckStop { size_t closest_idx{0}; size_t stuck_stopline_idx{0}; + std::string safety_report; }; /** - * @struct * @brief only collision is detected */ struct NonOccludedCollisionStop @@ -61,10 +67,10 @@ struct NonOccludedCollisionStop size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; + std::string safety_report; }; /** - * @struct * @brief occlusion is detected so ego needs to stop at the default stop line position */ struct FirstWaitBeforeOcclusion @@ -76,7 +82,6 @@ struct FirstWaitBeforeOcclusion }; /** - * @struct * @brief ego is approaching the boundary of attention area in the presence of traffic light */ struct PeekingTowardOcclusion @@ -96,7 +101,6 @@ struct PeekingTowardOcclusion }; /** - * @struct * @brief both collision and occlusion are detected in the presence of traffic light */ struct OccludedCollisionStop @@ -110,10 +114,10 @@ struct OccludedCollisionStop //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it //! contains the remaining time to release the static occlusion stuck std::optional static_occlusion_timeout{std::nullopt}; + std::string safety_report; }; /** - * @struct * @brief at least occlusion is detected in the absence of traffic light */ struct OccludedAbsenceTrafficLight @@ -124,10 +128,10 @@ struct OccludedAbsenceTrafficLight size_t closest_idx{0}; size_t first_attention_area_stopline_idx{0}; size_t peeking_limit_line_idx{0}; + std::string safety_report; }; /** - * @struct * @brief both collision and occlusion are not detected */ struct Safe @@ -138,7 +142,6 @@ struct Safe }; /** - * @struct * @brief traffic light is red or arrow signal */ struct FullyPrioritized @@ -147,10 +150,12 @@ struct FullyPrioritized size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; + std::string safety_report; }; using DecisionResult = std::variant< - Indecisive, //! internal process error, or over the pass judge line + InternalError, //! internal process error, or over the pass judge line + OverPassJudge, //! over the pass judge lines StuckStop, //! detected stuck vehicle YieldStuckStop, //! detected yield stuck vehicle NonOccludedCollisionStop, //! detected collision while FOV is clear @@ -162,41 +167,7 @@ using DecisionResult = std::variant< FullyPrioritized //! only detect vehicles violating traffic rules >; -inline std::string formatDecisionResult(const DecisionResult & decision_result) -{ - if (std::holds_alternative(decision_result)) { - const auto indecisive = std::get(decision_result); - return "Indecisive because " + indecisive.error; - } - if (std::holds_alternative(decision_result)) { - return "StuckStop"; - } - if (std::holds_alternative(decision_result)) { - return "YieldStuckStop"; - } - if (std::holds_alternative(decision_result)) { - return "NonOccludedCollisionStop"; - } - if (std::holds_alternative(decision_result)) { - return "FirstWaitBeforeOcclusion"; - } - if (std::holds_alternative(decision_result)) { - return "PeekingTowardOcclusion"; - } - if (std::holds_alternative(decision_result)) { - return "OccludedCollisionStop"; - } - if (std::holds_alternative(decision_result)) { - return "OccludedAbsenceTrafficLight"; - } - if (std::holds_alternative(decision_result)) { - return "Safe"; - } - if (std::holds_alternative(decision_result)) { - return "FullyPrioritized"; - } - return ""; -} +std::string formatDecisionResult(const DecisionResult & decision_result); } // namespace behavior_velocity_planner::intersection diff --git a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp index c47f016fbdbda..9002c88354d68 100644 --- a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp +++ b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp @@ -27,7 +27,6 @@ namespace behavior_velocity_planner::intersection { /** - * @struct * @brief wrapper class of interpolated path with lane id */ struct InterpolatedPathInfo diff --git a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp index 11deae6bdc001..9624d375de122 100644 --- a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp +++ b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp @@ -31,7 +31,6 @@ namespace behavior_velocity_planner::intersection { /** - * @struct * @brief see the document for more details of IntersectionLanelets */ struct IntersectionLanelets @@ -174,7 +173,6 @@ struct IntersectionLanelets }; /** - * @struct * @brief see the document for more details of PathLanelets */ struct PathLanelets diff --git a/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp b/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp index 64d20b81e3fad..99d79d4468b38 100644 --- a/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp +++ b/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp @@ -21,7 +21,6 @@ namespace behavior_velocity_planner::intersection { /** - * @struct * @brief see the document for more details of IntersectionStopLines */ struct IntersectionStopLines @@ -63,9 +62,9 @@ struct IntersectionStopLines /** * second_pass_judge_line is before second_attention_stopline by the braking distance. if - * second_attention_lane is null, it is same as first_pass_judge_line + * second_attention_lane is null, it is null */ - size_t second_pass_judge_line{0}; + std::optional second_pass_judge_line{std::nullopt}; /** * occlusion_wo_tl_pass_judge_line is null if ego footprint along the path does not intersect with diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 615991bc5e027..8ab67c810a30e 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -133,6 +133,11 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.collision_detection.ignore_on_red_traffic_light.object_margin_to_path = getOrDeclareParameter( node, ns + ".collision_detection.ignore_on_red_traffic_light.object_margin_to_path"); + ip.collision_detection.avoid_collision_by_acceleration + .object_time_margin_to_collision_point = getOrDeclareParameter( + node, + ns + + ".collision_detection.avoid_collision_by_acceleration.object_time_margin_to_collision_point"); ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_intersection_module/src/object_manager.cpp new file mode 100644 index 0000000000000..ea5d89fe8052d --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/object_manager.cpp @@ -0,0 +1,309 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "object_manager.hpp" + +#include +#include +#include // for toPolygon2d + +#include +#include +#include + +#include +#include + +#include + +namespace +{ +std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) +{ + std::stringstream ss; + for (auto i = 0; i < 16; ++i) { + ss << std::hex << std::setfill('0') << std::setw(2) << +uuid.uuid[i]; + } + return ss.str(); +} + +tier4_autoware_utils::Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, + const autoware_auto_perception_msgs::msg::Shape & shape) +{ + namespace bg = boost::geometry; + const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); + const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); + + tier4_autoware_utils::Polygon2d one_step_poly; + for (const auto & point : prev_poly.outer()) { + one_step_poly.outer().push_back(point); + } + for (const auto & point : next_poly.outer()) { + one_step_poly.outer().push_back(point); + } + + bg::correct(one_step_poly); + + tier4_autoware_utils::Polygon2d convex_one_step_poly; + bg::convex_hull(one_step_poly, convex_one_step_poly); + + return convex_one_step_poly; +} + +} // namespace + +namespace behavior_velocity_planner::intersection +{ +namespace bg = boost::geometry; + +ObjectInfo::ObjectInfo(const unique_identifier_msgs::msg::UUID & uuid) : uuid_str(::to_string(uuid)) +{ +} + +void ObjectInfo::initialize( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + std::optional attention_lanelet_opt_, + std::optional stopline_opt_) +{ + predicted_object_ = object; + attention_lanelet_opt = attention_lanelet_opt_; + stopline_opt = stopline_opt_; + unsafe_interval_ = std::nullopt; + calc_dist_to_stopline(); +} + +void ObjectInfo::update_safety( + const std::optional & unsafe_interval, + const std::optional & safe_interval, const bool safe_under_traffic_control) +{ + unsafe_interval_ = unsafe_interval; + safe_interval_ = safe_interval; + safe_under_traffic_control_ = safe_under_traffic_control; +} + +std::optional ObjectInfo::estimated_past_position( + const double past_duration) const +{ + if (!attention_lanelet_opt) { + return std::nullopt; + } + const auto attention_lanelet = attention_lanelet_opt.value(); + const auto current_arc_coords = lanelet::utils::getArcCoordinates( + {attention_lanelet}, predicted_object_.kinematics.initial_pose_with_covariance.pose); + const auto distance = current_arc_coords.distance; + const auto past_length = + current_arc_coords.length - + predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x * past_duration; + const auto past_point = lanelet::geometry::fromArcCoordinates( + attention_lanelet.centerline2d(), lanelet::ArcCoordinates{past_length, distance}); + geometry_msgs::msg::Point past_position; + past_position.x = past_point.x(); + past_position.y = past_point.y(); + return std::make_optional(past_position); +} + +void ObjectInfo::calc_dist_to_stopline() +{ + if (!stopline_opt || !attention_lanelet_opt) { + return; + } + const auto attention_lanelet = attention_lanelet_opt.value(); + const auto object_arc_coords = lanelet::utils::getArcCoordinates( + {attention_lanelet}, predicted_object_.kinematics.initial_pose_with_covariance.pose); + const auto stopline = stopline_opt.value(); + geometry_msgs::msg::Pose stopline_center; + stopline_center.position.x = (stopline.front().x() + stopline.back().x()) / 2.0; + stopline_center.position.y = (stopline.front().y() + stopline.back().y()) / 2.0; + stopline_center.position.z = (stopline.front().z() + stopline.back().z()) / 2.0; + const auto stopline_arc_coords = + lanelet::utils::getArcCoordinates({attention_lanelet}, stopline_center); + dist_to_stopline_opt = (stopline_arc_coords.length - object_arc_coords.length); +} + +bool ObjectInfo::can_stop_before_stopline(const double brake_deceleration) const +{ + if (!dist_to_stopline_opt) { + return false; + } + const double observed_velocity = + predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; + const double dist_to_stopline = dist_to_stopline_opt.value(); + const double braking_distance = + (observed_velocity * observed_velocity) / (2.0 * brake_deceleration); + return dist_to_stopline > braking_distance; +} + +bool ObjectInfo::can_stop_before_ego_lane( + const double brake_deceleration, const double tolerable_overshoot, + lanelet::ConstLanelet ego_lane) const +{ + if (!dist_to_stopline_opt || !stopline_opt || !attention_lanelet_opt) { + return false; + } + const double dist_to_stopline = dist_to_stopline_opt.value(); + const double observed_velocity = + predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = + (observed_velocity * observed_velocity) / (2.0 * brake_deceleration); + if (dist_to_stopline > braking_distance) { + return false; + } + const auto attention_lanelet = attention_lanelet_opt.value(); + const auto stopline = stopline_opt.value(); + const auto stopline_p1 = stopline.front(); + const auto stopline_p2 = stopline.back(); + const tier4_autoware_utils::Point2d stopline_mid{ + stopline_p1.x() + stopline_p2.x() / 2.0, (stopline_p1.y() + stopline_p2.y()) / 2.0}; + const auto attention_lane_end = attention_lanelet.centerline().back(); + const tier4_autoware_utils::LineString2d attention_lane_later_part( + {tier4_autoware_utils::Point2d{stopline_mid.x(), stopline_mid.y()}, + tier4_autoware_utils::Point2d{attention_lane_end.x(), attention_lane_end.y()}}); + std::vector ego_collision_points; + bg::intersection( + attention_lane_later_part, ego_lane.centerline2d().basicLineString(), ego_collision_points); + if (ego_collision_points.empty()) { + return false; + } + const auto expected_collision_point = ego_collision_points.front(); + // distance from object expected stop position to collision point + const double stopline_to_object = -1.0 * dist_to_stopline + braking_distance; + const double stopline_to_ego_path = std::hypot( + expected_collision_point.x() - stopline_mid.x(), + expected_collision_point.y() - stopline_mid.y()); + const double object_to_ego_path = stopline_to_ego_path - stopline_to_object; + // NOTE: if object_to_ego_path < 0, object passed ego path + return object_to_ego_path > tolerable_overshoot; +} + +bool ObjectInfo::before_stopline_by(const double margin) const +{ + if (!dist_to_stopline_opt) { + return false; + } + const double dist_to_stopline = dist_to_stopline_opt.value(); + return dist_to_stopline < margin; +} + +std::shared_ptr ObjectInfoManager::registerObject( + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, + const bool belong_intersection_area, const bool is_parked_vehicle) +{ + if (objects_info_.count(uuid) == 0) { + auto object = std::make_shared(uuid); + objects_info_[uuid] = object; + } + auto object = objects_info_[uuid]; + if (belong_attention_area) { + attention_area_objects_.push_back(object); + } else if (belong_intersection_area) { + intersection_area_objects_.push_back(object); + } + if (is_parked_vehicle) { + parked_objects_.push_back(object); + } + return object; +} + +void ObjectInfoManager::registerExistingObject( + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, + const bool belong_intersection_area, const bool is_parked_vehicle, + std::shared_ptr object) +{ + objects_info_[uuid] = object; + if (belong_attention_area) { + attention_area_objects_.push_back(object); + } else if (belong_intersection_area) { + intersection_area_objects_.push_back(object); + } + if (is_parked_vehicle) { + parked_objects_.push_back(object); + } +} + +void ObjectInfoManager::clearObjects() +{ + objects_info_.clear(); + attention_area_objects_.clear(); + intersection_area_objects_.clear(); + parked_objects_.clear(); +}; + +std::vector> ObjectInfoManager::allObjects() +{ + std::vector> all_objects = attention_area_objects_; + all_objects.insert( + all_objects.end(), intersection_area_objects_.begin(), intersection_area_objects_.end()); + all_objects.insert(all_objects.end(), parked_objects_.begin(), parked_objects_.end()); + return all_objects; +} + +std::optional findPassageInterval( + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_auto_perception_msgs::msg::Shape & shape, + const lanelet::BasicPolygon2d & ego_lane_poly, + const std::optional & first_attention_lane_opt, + const std::optional & second_attention_lane_opt) +{ + const auto first_itr = std::adjacent_find( + predicted_path.path.cbegin(), predicted_path.path.cend(), [&](const auto & a, const auto & b) { + return bg::intersects(ego_lane_poly, ::createOneStepPolygon(a, b, shape)); + }); + if (first_itr == predicted_path.path.cend()) { + // even the predicted path end does not collide with the beginning of ego_lane_poly + return std::nullopt; + } + const auto last_itr = std::adjacent_find( + predicted_path.path.crbegin(), predicted_path.path.crend(), + [&](const auto & a, const auto & b) { + return bg::intersects(ego_lane_poly, ::createOneStepPolygon(a, b, shape)); + }); + if (last_itr == predicted_path.path.crend()) { + // even the predicted path start does not collide with the end of ego_lane_poly + return std::nullopt; + } + + const size_t enter_idx = static_cast(first_itr - predicted_path.path.begin()); + const double object_enter_time = + static_cast(enter_idx) * rclcpp::Duration(predicted_path.time_step).seconds(); + const size_t exit_idx = std::distance(predicted_path.path.begin(), last_itr.base()) - 1; + const double object_exit_time = + static_cast(exit_idx) * rclcpp::Duration(predicted_path.time_step).seconds(); + const auto lane_position = [&]() { + if (first_attention_lane_opt) { + if (lanelet::geometry::inside( + first_attention_lane_opt.value(), + lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) { + return intersection::CollisionInterval::LanePosition::FIRST; + } + } + if (second_attention_lane_opt) { + if (lanelet::geometry::inside( + second_attention_lane_opt.value(), + lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) { + return intersection::CollisionInterval::LanePosition::SECOND; + } + } + return intersection::CollisionInterval::LanePosition::ELSE; + }(); + + std::vector path; + for (const auto & pose : predicted_path.path) { + path.push_back(pose); + } + return intersection::CollisionInterval{ + lane_position, path, {enter_idx, exit_idx}, {object_enter_time, object_exit_time}}; +} + +} // namespace behavior_velocity_planner::intersection diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.hpp b/planning/behavior_velocity_intersection_module/src/object_manager.hpp new file mode 100644 index 0000000000000..e77849570cda8 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/object_manager.hpp @@ -0,0 +1,294 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBJECT_MANAGER_HPP_ +#define OBJECT_MANAGER_HPP_ + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace std +{ +template <> +struct hash +{ + size_t operator()(const unique_identifier_msgs::msg::UUID & uid) const + { + const auto & ids = uid.uuid; + boost::uuids::uuid u = {ids[0], ids[1], ids[2], ids[3], ids[4], ids[5], ids[6], ids[7], + ids[8], ids[9], ids[10], ids[11], ids[12], ids[13], ids[14], ids[15]}; + return boost::hash()(u); + } +}; +} // namespace std + +namespace behavior_velocity_planner::intersection +{ + +/** + * @brief store collision information + */ +struct CollisionInterval +{ + enum LanePosition { + FIRST, + SECOND, + ELSE, + }; + LanePosition lane_position{LanePosition::ELSE}; + + //! original predicted path + std::vector path; + + //! possible collision interval position index on path + std::pair interval_position; + + //! possible collision interval time(without TTC margin) + std::pair interval_time; +}; + +struct CollisionKnowledge +{ + //! the time when the expected collision is judged + rclcpp::Time stamp; + + enum SafeType { + UNSAFE, + SAFE, + SAFE_UNDER_TRAFFIC_CONTROL, + }; + SafeType safe_type{SafeType::UNSAFE}; + + //! if !safe, this has value, and it safe, this maybe null if the predicted path does not + //! intersect with ego path + std::optional interval{std::nullopt}; + + double observed_velocity; +}; + +/** + * @brief store collision information of object on the attention area + */ +class ObjectInfo +{ +public: + explicit ObjectInfo(const unique_identifier_msgs::msg::UUID & uuid); + + const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object() const + { + return predicted_object_; + }; + + std::optional is_unsafe() const + { + if (safe_under_traffic_control_) { + return std::nullopt; + } + if (!unsafe_interval_) { + return std::nullopt; + } + return unsafe_interval_; + } + + bool is_safe_under_traffic_control() const { return safe_under_traffic_control_; } + + /** + * @brief update predicted_object_, attention_lanelet, stopline, dist_to_stopline + */ + void initialize( + const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object, + std::optional attention_lanelet_opt, + std::optional stopline_opt); + + /** + * @brief update unsafe_knowledge + */ + void update_safety( + const std::optional & unsafe_interval_opt, + const std::optional & safe_interval_opt, + const bool safe_under_traffic_control); + + /** + * @brief find the estimated position of the object in the past + */ + std::optional estimated_past_position( + const double past_duration) const; + + /** + * @brief check if object can stop before stopline under the deceleration. return false if + * stopline is null for conservative collision checking + */ + bool can_stop_before_stopline(const double brake_deceleration) const; + + /** + * @brief check if object can stop before stopline within the overshoot margin. return false if + * stopline is null for conservative collision checking + */ + bool can_stop_before_ego_lane( + const double brake_deceleration, const double tolerable_overshoot, + lanelet::ConstLanelet ego_lane) const; + + /** + * @brief check if the object is before the stopline within the specified margin + */ + bool before_stopline_by(const double margin) const; + + void setDecisionAt1stPassJudgeLinePassage(const CollisionKnowledge & knowledge) + { + decision_at_1st_pass_judge_line_passage_ = knowledge; + } + + void setDecisionAt2ndPassJudgeLinePassage(const CollisionKnowledge & knowledge) + { + decision_at_2nd_pass_judge_line_passage_ = knowledge; + } + + const std::optional & unsafe_interval() const { return unsafe_interval_; } + + double observed_velocity() const + { + return predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; + } + + const std::optional & decision_at_1st_pass_judge_line_passage() const + { + return decision_at_1st_pass_judge_line_passage_; + } + + const std::optional & decision_at_2nd_pass_judge_line_passage() const + { + return decision_at_2nd_pass_judge_line_passage_; + } + + const std::string uuid_str; + +private: + autoware_auto_perception_msgs::msg::PredictedObject predicted_object_; + + //! null if the object in intersection_area but not in attention_area + std::optional attention_lanelet_opt{std::nullopt}; + + //! null if the object in intersection_area but not in attention_area + std::optional stopline_opt{std::nullopt}; + + //! null if the object in intersection_area but not in attention_area + std::optional dist_to_stopline_opt{std::nullopt}; + + //! store the information if judged as UNSAFE + std::optional unsafe_interval_{std::nullopt}; + + //! store the information if judged as SAFE + std::optional safe_interval_{std::nullopt}; + + //! true if the object is judged as negligible given traffic light color + bool safe_under_traffic_control_{false}; + + std::optional decision_at_1st_pass_judge_line_passage_{std::nullopt}; + std::optional decision_at_2nd_pass_judge_line_passage_{std::nullopt}; + + /** + * @brief calculate/update the distance to corresponding stopline + */ + void calc_dist_to_stopline(); +}; + +/** + * @brief store predicted objects for intersection + */ +class ObjectInfoManager +{ +public: + std::shared_ptr registerObject( + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, + const bool belong_intersection_area, const bool is_parked); + + void registerExistingObject( + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, + const bool belong_intersection_area, const bool is_parked, + std::shared_ptr object); + + void clearObjects(); + + const std::vector> & attentionObjects() const + { + return attention_area_objects_; + } + + const std::vector> & parkedObjects() const { return parked_objects_; } + + std::vector> allObjects(); + + const std::unordered_map> & + getObjectsMap() + { + return objects_info_; + } + + void setPassed1stPassJudgeLineFirstTime(const rclcpp::Time & time) + { + passed_1st_judge_line_first_time_ = time; + } + void setPassed2ndPassJudgeLineFirstTime(const rclcpp::Time & time) + { + passed_2nd_judge_line_first_time_ = time; + } + +private: + std::unordered_map> objects_info_; + + //! belong to attention area + std::vector> attention_area_objects_; + + //! does not belong to attention area but to intersection area + std::vector> intersection_area_objects_; + + //! parked objects on attention_area/intersection_area + std::vector> parked_objects_; + + std::optional passed_1st_judge_line_first_time_{std::nullopt}; + std::optional passed_2nd_judge_line_first_time_{std::nullopt}; +}; + +/** + * @brief return the CollisionInterval struct if the predicted path collides ego path geometrically + */ +std::optional findPassageInterval( + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_auto_perception_msgs::msg::Shape & shape, + const lanelet::BasicPolygon2d & ego_lane_poly, + const std::optional & first_attention_lane_opt, + const std::optional & second_attention_lane_opt); + +} // namespace behavior_velocity_planner::intersection + +#endif // OBJECT_MANAGER_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/result.hpp b/planning/behavior_velocity_intersection_module/src/result.hpp index cc6ad880b8153..5d82183cee2fb 100644 --- a/planning/behavior_velocity_intersection_module/src/result.hpp +++ b/planning/behavior_velocity_intersection_module/src/result.hpp @@ -15,6 +15,7 @@ #ifndef RESULT_HPP_ #define RESULT_HPP_ +#include #include namespace behavior_velocity_planner::intersection @@ -23,10 +24,6 @@ namespace behavior_velocity_planner::intersection template class Result { -public: - static Result make_ok(const Ok & ok) { return Result(ok); } - static Result make_err(const Error & err) { return Result(err); } - public: explicit Result(const Ok & ok) : data_(ok) {} explicit Result(const Error & err) : data_(err) {} @@ -39,6 +36,18 @@ class Result std::variant data_; }; +template +Result make_ok(Args &&... args) +{ + return Result(Ok{std::forward(args)...}); +} + +template +Result make_err(Args &&... args) +{ + return Result(Error{std::forward(args)...}); +} + } // namespace behavior_velocity_planner::intersection #endif // RESULT_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 90242dc3edd7e..3f6136673a44a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -40,6 +40,10 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; +using intersection::make_err; +using intersection::make_ok; +using intersection::Result; + IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, @@ -95,10 +99,8 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * debug_data_ = DebugData(); *stop_reason = planning_utils::initializeStopReason(StopReason::INTERSECTION); - // set default RTC initializeRTCStatus(); - // calculate the const auto decision_result = modifyPathVelocityDetail(path, stop_reason); prev_decision_result_ = decision_result; @@ -112,7 +114,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * reactRTCApproval(decision_result, path, stop_reason); - RCLCPP_DEBUG(logger_, "===== plan end ====="); return true; } @@ -141,133 +142,145 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto [interpolated_path_info, intersection_stoplines, path_lanelets] = prepare_data.ok(); const auto & intersection_lanelets = intersection_lanelets_.value(); - const auto closest_idx = intersection_stoplines.closest_idx; - - // utility functions - auto fromEgoDist = [&](const size_t index) { - return motion_utils::calcSignedArcLength(path->points, closest_idx, index); - }; - auto stoppedForDuration = - [&](const size_t pos, const double duration, StateMachine & state_machine) { - const double dist_stopline = fromEgoDist(pos); - const bool approached_dist_stopline = - (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); - const bool over_stopline = (dist_stopline < 0.0); - const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - state_machine.setState(StateMachine::State::GO); - } else if (is_stopped_duration && approached_dist_stopline) { - state_machine.setState(StateMachine::State::GO); - } - return state_machine.getState() == StateMachine::State::GO; - }; - auto stoppedAtPosition = [&](const size_t pos, const double duration) { - const double dist_stopline = fromEgoDist(pos); - const bool approached_dist_stopline = - (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); - const bool over_stopline = (dist_stopline < -planner_param_.common.stopline_overshoot_margin); - const bool is_stopped = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - return true; - } else if (is_stopped && approached_dist_stopline) { - return true; - } - return false; - }; - + // ========================================================================================== + // stuck detection + // // stuck vehicle detection is viable even if attention area is empty // so this needs to be checked before attention area validation + // ========================================================================================== const auto is_stuck_status = isStuckStatus(*path, intersection_stoplines, path_lanelets); if (is_stuck_status) { return is_stuck_status.value(); } + // ========================================================================================== + // basic data validation + // // if attention area is empty, collision/occlusion detection is impossible + // + // if attention area is not null but default stop line is not available, ego/backward-path has + // already passed the stop line, so ego is already in the middle of the intersection, or the + // end of the ego path has just entered the entry of this intersection + // + // occlusion stop line is generated from the intersection of ego footprint along the path with the + // attention area, so if this is null, ego has already passed the intersection, or the end of the + // ego path has just entered the entry of this intersection + // ========================================================================================== if (!intersection_lanelets.first_attention_area()) { - return intersection::Indecisive{"attention area is empty"}; + return intersection::InternalError{"attention area is empty"}; } const auto first_attention_area = intersection_lanelets.first_attention_area().value(); - // if attention area is not null but default stop line is not available, ego/backward-path has - // already passed the stop line const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; if (!default_stopline_idx_opt) { - return intersection::Indecisive{"default stop line is null"}; + return intersection::InternalError{"default stop line is null"}; } const auto default_stopline_idx = default_stopline_idx_opt.value(); - // occlusion stop line is generated from the intersection of ego footprint along the path with the - // attention area, so if this is null, eog has already passed the intersection const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; if (!first_attention_stopline_idx_opt || !occlusion_peeking_stopline_idx_opt) { - return intersection::Indecisive{"occlusion stop line is null"}; + return intersection::InternalError{"occlusion stop line is null"}; } const auto first_attention_stopline_idx = first_attention_stopline_idx_opt.value(); const auto occlusion_stopline_idx = occlusion_peeking_stopline_idx_opt.value(); - debug_data_.attention_area = intersection_lanelets.attention_area(); - debug_data_.first_attention_area = intersection_lanelets.first_attention_area(); - debug_data_.second_attention_area = intersection_lanelets.second_attention_area(); - debug_data_.occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); - debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); + // ========================================================================================== + // classify the objects to attention_area/intersection_area and update their position, velocity, + // belonging attention lanelet, distance to corresponding stopline + // ========================================================================================== + updateObjectInfoManagerArea(); + + // ========================================================================================== + // occlusion_status is type of occlusion, + // is_occlusion_cleared_with_margin indicates if occlusion is physically detected + // is_occlusion_state indicates if occlusion is detected. OR occlusion is not detected but RTC for + // intersection_occlusion is disapproved, which means ego is virtually occluded + // + // so is_occlusion_cleared_with_margin should be sent to RTC as module decision + // and is_occlusion_status should be only used to decide ego action + // !is_occlusion_state == !physically_occluded && !externally_occluded, so even if occlusion is + // not detected but not approved, SAFE is not sent. + // ========================================================================================== + const auto [occlusion_status, is_occlusion_cleared_with_margin, is_occlusion_state] = + getOcclusionStatus(traffic_prioritized_level, interpolated_path_info); - // get intersection area - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); - const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); - const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); - // filter objects - auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); - const auto is_yield_stuck_status = - isYieldStuckStatus(*path, interpolated_path_info, intersection_stoplines, target_objects); - if (is_yield_stuck_status) { - return is_yield_stuck_status.value(); + const auto + [is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line, safely_passed_1st_judge_line, + safely_passed_2nd_judge_line] = + isOverPassJudgeLinesStatus(*path, is_occlusion_state, intersection_stoplines); + + // ========================================================================================== + // calculate the expected vehicle speed and obtain the spatiotemporal profile of ego to the + // exit of intersection + // ========================================================================================== + tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; + const auto time_distance_array = + calcIntersectionPassingTime(*path, is_prioritized, intersection_stoplines, &ego_ttc_time_array); + + // ========================================================================================== + // run collision checking for each objects considering traffic light level. Also if ego just + // passed each pass judge line for the first time, save current collision status for late + // diagnosis + // ========================================================================================== + updateObjectInfoManagerCollision( + path_lanelets, time_distance_array, traffic_prioritized_level, safely_passed_1st_judge_line, + safely_passed_2nd_judge_line); + + const auto [has_collision, collision_position, too_late_detect_objects, misjudge_objects] = + detectCollision(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line); + const std::string safety_diag = + generateDetectionBlameDiagnosis(too_late_detect_objects, misjudge_objects); + if (is_permanent_go_) { + if (has_collision) { + const auto closest_idx = intersection_stoplines.closest_idx; + const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis( + *path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects); + return intersection::OverPassJudge{safety_diag, evasive_diag}; + } + return intersection::OverPassJudge{ + "no collision is detected", "ego can safely pass the intersection at this rate"}; } - const auto [occlusion_status, is_occlusion_cleared_with_margin, is_occlusion_state] = - getOcclusionStatus( - traffic_prioritized_level, interpolated_path_info, intersection_lanelets, target_objects); - - // TODO(Mamoru Sobue): this should be called later for safety diagnosis - const auto is_over_pass_judge_lines_status = - isOverPassJudgeLinesStatus(*path, is_occlusion_state, intersection_stoplines); - if (is_over_pass_judge_lines_status) { - return is_over_pass_judge_lines_status.ok(); + const bool collision_on_1st_attention_lane = + has_collision && (collision_position == intersection::CollisionInterval::LanePosition::FIRST); + // ========================================================================================== + // this state is very dangerous because ego is very close/over the boundary of 1st attention lane + // and collision is detected on the 1st lane. Since the 2nd attention lane also exists in this + // case, possible another collision may be expected on the 2nd attention lane too. + // ========================================================================================== + std::string safety_report = safety_diag; + if ( + is_over_1st_pass_judge_line && is_over_2nd_pass_judge_line && + is_over_2nd_pass_judge_line.value() && collision_on_1st_attention_lane) { + safety_report += + "\nego is between the 1st and 2nd pass judge line but collision is expected on the 1st " + "attention lane, which is dangerous."; } - [[maybe_unused]] const auto [is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line] = - is_over_pass_judge_lines_status.err(); - const auto & current_pose = planner_data_->current_odometry->pose; - const bool is_over_default_stopline = - util::isOverTargetIndex(*path, closest_idx, current_pose, default_stopline_idx); + const auto closest_idx = intersection_stoplines.closest_idx; + const bool is_over_default_stopline = util::isOverTargetIndex( + *path, closest_idx, planner_data_->current_odometry->pose, default_stopline_idx); const auto collision_stopline_idx = is_over_default_stopline ? closest_idx : default_stopline_idx; - const auto is_green_pseudo_collision_status = isGreenPseudoCollisionStatus( - *path, collision_stopline_idx, intersection_stoplines, target_objects); + // ========================================================================================== + // pseudo collision detection on green light + // ========================================================================================== + const auto is_green_pseudo_collision_status = + isGreenPseudoCollisionStatus(closest_idx, collision_stopline_idx, intersection_stoplines); if (is_green_pseudo_collision_status) { return is_green_pseudo_collision_status.value(); } - // if ego is waiting for collision detection, the entry time into the intersection is a bit - // delayed for the chattering hold - const bool is_go_out = (activated_ && occlusion_activated_); - const double time_to_restart = - (is_go_out || is_prioritized) - ? 0.0 - : (planner_param_.collision_detection.collision_detection_hold_time - - collision_state_machine_.getDuration()); - - // TODO(Mamoru Sobue): if ego is over 1st/2nd pass judge line and collision is expected at 1st/2nd - // pass judge line respectively, ego should go - const auto second_attention_stopline_idx = intersection_stoplines.second_attention_stopline; - const auto last_intersection_stopline_candidate_idx = - second_attention_stopline_idx ? second_attention_stopline_idx.value() : occlusion_stopline_idx; - tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; - const auto time_distance_array = calcIntersectionPassingTime( - *path, closest_idx, last_intersection_stopline_candidate_idx, time_to_restart, - &ego_ttc_time_array); + // ========================================================================================== + // yield stuck detection + // ========================================================================================== + const auto is_yield_stuck_status = + isYieldStuckStatus(*path, interpolated_path_info, intersection_stoplines); + if (is_yield_stuck_status) { + auto yield_stuck = is_yield_stuck_status.value(); + yield_stuck.safety_report = safety_report; + return yield_stuck; + } - const bool has_collision = checkCollision( - *path, &target_objects, path_lanelets, closest_idx, last_intersection_stopline_candidate_idx, - time_to_restart, traffic_prioritized_level); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("collision state_machine"), *clock_); @@ -276,7 +289,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( if (is_prioritized) { return intersection::FullyPrioritized{ - has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx, + safety_report}; } // Safe @@ -286,18 +300,51 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( // Only collision if (!is_occlusion_state && has_collision_with_margin) { return intersection::NonOccludedCollisionStop{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + closest_idx, collision_stopline_idx, occlusion_stopline_idx, safety_report}; } // Occluded - // occlusion_status is assured to be not NOT_OCCLUDED + // utility functions + auto fromEgoDist = [&](const size_t index) { + return motion_utils::calcSignedArcLength(path->points, closest_idx, index); + }; + auto stoppedForDuration = + [&](const size_t pos, const double duration, StateMachine & state_machine) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = + (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); + const bool over_stopline = (dist_stopline < 0.0); + const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + state_machine.setState(StateMachine::State::GO); + } else if (is_stopped_duration && approached_dist_stopline) { + state_machine.setState(StateMachine::State::GO); + } + return state_machine.getState() == StateMachine::State::GO; + }; + auto stoppedAtPosition = [&](const size_t pos, const double duration) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = + (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); + const bool over_stopline = (dist_stopline < -planner_param_.common.stopline_overshoot_margin); + const bool is_stopped = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + return true; + } else if (is_stopped && approached_dist_stopline) { + return true; + } + return false; + }; + const auto occlusion_wo_tl_pass_judge_line_idx = intersection_stoplines.occlusion_wo_tl_pass_judge_line; const bool stopped_at_default_line = stoppedForDuration( default_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking, before_creep_state_machine_); if (stopped_at_default_line) { + // ========================================================================================== // if specified the parameter occlusion.temporal_stop_before_attention_area OR // has_no_traffic_light_, ego will temporarily stop before entering attention area + // ========================================================================================== const bool temporal_stop_before_attention_required = (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) ? !stoppedForDuration( @@ -307,8 +354,18 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( : false; if (!has_traffic_light_) { if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) { - return intersection::Indecisive{ - "already passed maximum peeking line in the absence of traffic light"}; + if (has_collision) { + const auto closest_idx = intersection_stoplines.closest_idx; + const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis( + *path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects); + return intersection::OverPassJudge{ + "already passed maximum peeking line in the absence of traffic light.\n" + + safety_report, + evasive_diag}; + } + return intersection::OverPassJudge{ + "already passed maximum peeking line in the absence of traffic light safely", + "no evasive action required"}; } return intersection::OccludedAbsenceTrafficLight{ is_occlusion_cleared_with_margin, @@ -316,10 +373,15 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( temporal_stop_before_attention_required, closest_idx, first_attention_stopline_idx, - occlusion_wo_tl_pass_judge_line_idx}; + occlusion_wo_tl_pass_judge_line_idx, + safety_diag}; } + + // ========================================================================================== // following remaining block is "has_traffic_light_" + // // if ego is stuck by static occlusion in the presence of traffic light, start timeout count + // ========================================================================================== const bool is_static_occlusion = occlusion_status == OcclusionType::STATICALLY_OCCLUDED; const bool is_stuck_by_static_occlusion = stoppedAtPosition( @@ -355,7 +417,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_stopline_idx, first_attention_stopline_idx, occlusion_stopline_idx, - static_occlusion_timeout}; + static_occlusion_timeout, + safety_report}; } else { return intersection::PeekingTowardOcclusion{ is_occlusion_cleared_with_margin, @@ -398,7 +461,17 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - [[maybe_unused]] const intersection::Indecisive & result, + [[maybe_unused]] const intersection::InternalError & result, + [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, + [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) +{ + return; +} + +template <> +void prepareRTCByDecisionResult( + [[maybe_unused]] const intersection::OverPassJudge & result, [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) @@ -606,7 +679,22 @@ template <> void reactRTCApprovalByDecisionResult( [[maybe_unused]] const bool rtc_default_approved, [[maybe_unused]] const bool rtc_occlusion_approved, - [[maybe_unused]] const intersection::Indecisive & decision_result, + [[maybe_unused]] const intersection::InternalError & decision_result, + [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, + [[maybe_unused]] const double baselink2front, + [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] StopReason * stop_reason, + [[maybe_unused]] VelocityFactorInterface * velocity_factor, + [[maybe_unused]] IntersectionModule::DebugData * debug_data) +{ + return; +} + +template <> +void reactRTCApprovalByDecisionResult( + [[maybe_unused]] const bool rtc_default_approved, + [[maybe_unused]] const bool rtc_occlusion_approved, + [[maybe_unused]] const intersection::OverPassJudge & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, @@ -640,7 +728,7 @@ void reactRTCApprovalByDecisionResult( { tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets); + stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->unsafe_targets); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(closest_idx).point.pose, @@ -689,7 +777,7 @@ void reactRTCApprovalByDecisionResult( { tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets); + stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->unsafe_targets); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(closest_idx).point.pose, @@ -807,7 +895,6 @@ void reactRTCApprovalByDecisionResult( "PeekingTowardOcclusion, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); // NOTE: creep_velocity should be inserted first at closest_idx if !rtc_default_approved - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { const size_t occlusion_peeking_stopline = decision_result.temporal_stop_before_attention_required @@ -1125,84 +1212,7 @@ IntersectionModule::TrafficPrioritizedLevel IntersectionModule::getTrafficPriori return TrafficPrioritizedLevel::NOT_PRIORITIZED; } -TargetObjects IntersectionModule::generateTargetObjects( - const intersection::IntersectionLanelets & intersection_lanelets, - const std::optional & intersection_area) const -{ - const auto & objects_ptr = planner_data_->predicted_objects; - // extract target objects - TargetObjects target_objects; - target_objects.header = objects_ptr->header; - const auto & attention_lanelets = intersection_lanelets.attention(); - const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stoplines(); - const auto & adjacent_lanelets = intersection_lanelets.adjacent(); - for (const auto & object : objects_ptr->objects) { - // ignore non-vehicle type objects, such as pedestrian. - if (!isTargetCollisionVehicleType(object)) { - continue; - } - - // check direction of objects - const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); - const auto belong_adjacent_lanelet_id = - checkAngleForTargetLanelets(object_direction, adjacent_lanelets, false); - if (belong_adjacent_lanelet_id) { - continue; - } - - const auto is_parked_vehicle = - std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x) < - planner_param_.occlusion.ignore_parked_vehicle_speed_threshold; - auto & container = is_parked_vehicle ? target_objects.parked_attention_objects - : target_objects.attention_objects; - if (intersection_area) { - const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; - const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); - const auto intersection_area_2d = intersection_area.value(); - const auto belong_attention_lanelet_id = - checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle); - if (belong_attention_lanelet_id) { - const auto id = belong_attention_lanelet_id.value(); - TargetObject target_object; - target_object.object = object; - target_object.attention_lanelet = attention_lanelets.at(id); - target_object.stopline = attention_lanelet_stoplines.at(id); - container.push_back(target_object); - } else if (bg::within(Point2d{obj_pos.x, obj_pos.y}, intersection_area_2d)) { - TargetObject target_object; - target_object.object = object; - target_object.attention_lanelet = std::nullopt; - target_object.stopline = std::nullopt; - target_objects.intersection_area_objects.push_back(target_object); - } - } else if (const auto belong_attention_lanelet_id = checkAngleForTargetLanelets( - object_direction, attention_lanelets, is_parked_vehicle); - belong_attention_lanelet_id.has_value()) { - // intersection_area is not available, use detection_area_with_margin as before - const auto id = belong_attention_lanelet_id.value(); - TargetObject target_object; - target_object.object = object; - target_object.attention_lanelet = attention_lanelets.at(id); - target_object.stopline = attention_lanelet_stoplines.at(id); - container.push_back(target_object); - } - } - for (const auto & object : target_objects.attention_objects) { - target_objects.all_attention_objects.push_back(object); - } - for (const auto & object : target_objects.parked_attention_objects) { - target_objects.all_attention_objects.push_back(object); - } - for (auto & object : target_objects.all_attention_objects) { - object.calc_dist_to_stopline(); - } - return target_objects; -} - -intersection::Result< - intersection::Indecisive, - std::pair> -IntersectionModule::isOverPassJudgeLinesStatus( +IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, const intersection::IntersectionStopLines & intersection_stoplines) { @@ -1216,8 +1226,11 @@ IntersectionModule::isOverPassJudgeLinesStatus( const size_t pass_judge_line_idx = [&]() { if (planner_param_.occlusion.enable) { if (has_traffic_light_) { + // ========================================================================================== // if ego passed the first_pass_judge_line while it is peeking to occlusion, then its - // position is occlusion_stopline_idx. Otherwise it is first_pass_judge_line + // position is changed to occlusion_stopline_idx. even if occlusion is cleared by peeking, + // its position should be occlusion_stopline_idx as before + // ========================================================================================== if (passed_1st_judge_line_while_peeking_) { return occlusion_stopline_idx; } @@ -1226,167 +1239,108 @@ IntersectionModule::isOverPassJudgeLinesStatus( if (is_occlusion_state && is_over_first_pass_judge_line) { passed_1st_judge_line_while_peeking_ = true; return occlusion_stopline_idx; - } else { - return first_pass_judge_line_idx; } + // ========================================================================================== + // Otherwise it is first_pass_judge_line + // ========================================================================================== + return first_pass_judge_line_idx; } else if (is_occlusion_state) { + // ========================================================================================== // if there is no traffic light and occlusion is detected, pass_judge position is beyond // the boundary of first attention area + // ========================================================================================== return occlusion_wo_tl_pass_judge_line_idx; } else { + // ========================================================================================== // if there is no traffic light and occlusion is not detected, pass_judge position is - // default + // default position + // ========================================================================================== return first_pass_judge_line_idx; } } return first_pass_judge_line_idx; }(); - const bool was_safe = std::holds_alternative(prev_decision_result_); + // ========================================================================================== + // at intersection without traffic light, this module ignores occlusion even if occlusion is + // detected for real, so if collision is not detected in that context, that should be interpreted + // as "was_safe" + // ========================================================================================== + const bool was_safe = [&]() { + if (std::holds_alternative(prev_decision_result_)) { + return true; + } + if (std::holds_alternative(prev_decision_result_)) { + const auto & state = + std::get(prev_decision_result_); + return !state.collision_detected; + } + return false; + }(); const bool is_over_1st_pass_judge_line = util::isOverTargetIndex(path, closest_idx, current_pose, pass_judge_line_idx); + bool safely_passed_1st_judge_line_first_time = false; if (is_over_1st_pass_judge_line && was_safe && !safely_passed_1st_judge_line_time_) { - safely_passed_1st_judge_line_time_ = clock_->now(); + safely_passed_1st_judge_line_time_ = std::make_pair(clock_->now(), current_pose); + safely_passed_1st_judge_line_first_time = true; } const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; debug_data_.first_pass_judge_wall_pose = planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, path); debug_data_.passed_first_pass_judge = safely_passed_1st_judge_line_time_.has_value(); - const auto second_pass_judge_line_idx = intersection_stoplines.second_pass_judge_line; - const bool is_over_2nd_pass_judge_line = - util::isOverTargetIndex(path, closest_idx, current_pose, second_pass_judge_line_idx); - if (is_over_2nd_pass_judge_line && was_safe && !safely_passed_2nd_judge_line_time_) { - safely_passed_2nd_judge_line_time_ = clock_->now(); + const auto second_pass_judge_line_idx_opt = intersection_stoplines.second_pass_judge_line; + const std::optional is_over_2nd_pass_judge_line = + second_pass_judge_line_idx_opt + ? std::make_optional(util::isOverTargetIndex( + path, closest_idx, current_pose, second_pass_judge_line_idx_opt.value())) + : std::nullopt; + bool safely_passed_2nd_judge_line_first_time = false; + if ( + is_over_2nd_pass_judge_line && is_over_2nd_pass_judge_line.value() && was_safe && + !safely_passed_2nd_judge_line_time_) { + safely_passed_2nd_judge_line_time_ = std::make_pair(clock_->now(), current_pose); + safely_passed_2nd_judge_line_first_time = true; + } + if (second_pass_judge_line_idx_opt) { + debug_data_.second_pass_judge_wall_pose = + planning_utils::getAheadPose(second_pass_judge_line_idx_opt.value(), baselink2front, path); } - debug_data_.second_pass_judge_wall_pose = - planning_utils::getAheadPose(second_pass_judge_line_idx, baselink2front, path); debug_data_.passed_second_pass_judge = safely_passed_2nd_judge_line_time_.has_value(); const bool is_over_default_stopline = util::isOverTargetIndex(path, closest_idx, current_pose, default_stopline_idx); + + const bool over_default_stopline_for_pass_judge = + is_over_default_stopline || planner_param_.common.enable_pass_judge_before_default_stopline; + const bool over_pass_judge_line_overall = + is_over_2nd_pass_judge_line ? is_over_2nd_pass_judge_line.value() : is_over_1st_pass_judge_line; if ( - ((is_over_default_stopline || - planner_param_.common.enable_pass_judge_before_default_stopline) && - is_over_2nd_pass_judge_line && was_safe) || + (over_default_stopline_for_pass_judge && over_pass_judge_line_overall && was_safe) || is_permanent_go_) { - /* - * This body is active if ego is - * - over the default stopline AND - * - over the 1st && 2nd pass judge line AND - * - previously safe - * , - * which means ego can stop even if it is over the 1st pass judge line but - * - before default stopline OR - * - before the 2nd pass judge line OR - * - or previously unsafe - * . - * In order for ego to continue peeking or collision detection when occlusion is detected after - * ego passed the 1st pass judge line, it needs to be - * - before the default stopline OR - * - before the 2nd pass judge line OR - * - previously unsafe - */ + // ========================================================================================== + // this body is active if ego is + // - over the default stopline AND + // - over the 1st && 2nd pass judge line AND + // - previously safe + // , + // which means ego can stop even if it is over the 1st pass judge line but + // - before default stopline OR + // - before the 2nd pass judge line OR + // - or previously unsafe + // . + // + // in order for ego to continue peeking or collision detection when occlusion is detected + // after ego passed the 1st pass judge line, it needs to be + // - before the default stopline OR + // - before the 2nd pass judge line OR + // - previously unsafe + // ========================================================================================== is_permanent_go_ = true; - return intersection::Result>::make_ok( - intersection::Indecisive{"over the pass judge line. no plan needed"}); - } - return intersection::Result>::make_err( - std::make_pair(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line)); -} - -void TargetObject::calc_dist_to_stopline() -{ - if (!attention_lanelet || !stopline) { - return; } - const auto attention_lanelet_val = attention_lanelet.value(); - const auto object_arc_coords = lanelet::utils::getArcCoordinates( - {attention_lanelet_val}, object.kinematics.initial_pose_with_covariance.pose); - const auto stopline_val = stopline.value(); - geometry_msgs::msg::Pose stopline_center; - stopline_center.position.x = (stopline_val.front().x() + stopline_val.back().x()) / 2.0; - stopline_center.position.y = (stopline_val.front().y() + stopline_val.back().y()) / 2.0; - stopline_center.position.z = (stopline_val.front().z() + stopline_val.back().z()) / 2.0; - const auto stopline_arc_coords = - lanelet::utils::getArcCoordinates({attention_lanelet_val}, stopline_center); - dist_to_stopline = (stopline_arc_coords.length - object_arc_coords.length); + return { + is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line, + safely_passed_1st_judge_line_first_time, safely_passed_2nd_judge_line_first_time}; } -/* - bool IntersectionModule::checkFrontVehicleDeceleration( - lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, - const Polygon2d & stuck_vehicle_detect_area, - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const double assumed_front_car_decel) - { - const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; - // consider vehicle in ego-lane && in front of ego - const auto lon_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; - const double object_decel = - planner_param_.stuck_vehicle.assumed_front_car_decel; // NOTE: this is positive - const double stopping_distance = lon_vel * lon_vel / (2 * object_decel); - - std::vector center_points; - for (auto && p : ego_lane_with_next_lane[0].centerline()) - center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); - for (auto && p : ego_lane_with_next_lane[1].centerline()) - center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); - const double lat_offset = - std::fabs(motion_utils::calcLateralOffset(center_points, object_pose.position)); - // get the nearest centerpoint to object - std::vector dist_obj_center_points; - for (const auto & p : center_points) - dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position, - p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_points.begin(), - std::min_element(dist_obj_center_points.begin(), dist_obj_center_points.end())); - // find two center_points whose distances from `closest_centerpoint` cross stopping_distance - double acc_dist_prev = 0.0, acc_dist = 0.0; - auto p1 = center_points[obj_closest_centerpoint_idx]; - auto p2 = center_points[obj_closest_centerpoint_idx]; - for (unsigned i = obj_closest_centerpoint_idx; i < center_points.size() - 1; ++i) { - p1 = center_points[i]; - p2 = center_points[i + 1]; - acc_dist_prev = acc_dist; - const auto arc_position_p1 = - lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p1)); - const auto arc_position_p2 = - lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p2)); - const double delta = arc_position_p2.length - arc_position_p1.length; - acc_dist += delta; - if (acc_dist > stopping_distance) { - break; - } - } - // if stopping_distance >= center_points, stopping_point is center_points[end] - const double ratio = (acc_dist <= stopping_distance) - ? 0.0 - : (acc_dist - stopping_distance) / (stopping_distance - acc_dist_prev); - // linear interpolation - geometry_msgs::msg::Point stopping_point; - stopping_point.x = (p1.x * ratio + p2.x) / (1 + ratio); - stopping_point.y = (p1.y * ratio + p2.y) / (1 + ratio); - stopping_point.z = (p1.z * ratio + p2.z) / (1 + ratio); - const double lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, stopping_point); - stopping_point.x += lat_offset * std::cos(lane_yaw + M_PI / 2.0); - stopping_point.y += lat_offset * std::sin(lane_yaw + M_PI / 2.0); - - // calculate footprint of predicted stopping pose - autoware_auto_perception_msgs::msg::PredictedObject predicted_object = object; - predicted_object.kinematics.initial_pose_with_covariance.pose.position = stopping_point; - predicted_object.kinematics.initial_pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); - auto predicted_obj_footprint = tier4_autoware_utils::toPolygon2d(predicted_object); - const bool is_in_stuck_area = !bg::disjoint(predicted_obj_footprint, stuck_vehicle_detect_area); - debug_data_.predicted_obj_pose.position = stopping_point; - debug_data_.predicted_obj_pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); - - if (is_in_stuck_area) { - return true; - } - return false; - } -*/ - } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 8a34aabe8b918..9527ce8e5ebea 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -19,6 +19,7 @@ #include "interpolated_path_info.hpp" #include "intersection_lanelets.hpp" #include "intersection_stoplines.hpp" +#include "object_manager.hpp" #include "result.hpp" #include @@ -45,32 +46,6 @@ namespace behavior_velocity_planner { -/** - * @struct - * @brief see the document for more details of IntersectionStopLines - */ -struct TargetObject -{ - autoware_auto_perception_msgs::msg::PredictedObject object; - std::optional attention_lanelet{std::nullopt}; - std::optional stopline{std::nullopt}; - std::optional dist_to_stopline{std::nullopt}; - void calc_dist_to_stopline(); -}; - -/** - * @struct - * @brief categorize TargetObjects - */ -struct TargetObjects -{ - std_msgs::msg::Header header; - std::vector attention_objects; - std::vector parked_attention_objects; - std::vector intersection_area_objects; - std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy -}; - class IntersectionModule : public SceneModuleInterface { public: @@ -154,6 +129,10 @@ class IntersectionModule : public SceneModuleInterface { double object_margin_to_path; } ignore_on_red_traffic_light; + struct AvoidCollisionByAcceleration + { + double object_time_margin_to_collision_point; + } avoid_collision_by_acceleration; } collision_detection; struct Occlusion @@ -204,36 +183,37 @@ class IntersectionModule : public SceneModuleInterface std::optional occlusion_stop_wall_pose{std::nullopt}; std::optional occlusion_first_stop_wall_pose{std::nullopt}; std::optional first_pass_judge_wall_pose{std::nullopt}; + std::optional second_pass_judge_wall_pose{std::nullopt}; bool passed_first_pass_judge{false}; bool passed_second_pass_judge{false}; - std::optional second_pass_judge_wall_pose{std::nullopt}; + std::optional absence_traffic_light_creep_wall{std::nullopt}; + std::optional> attention_area{std::nullopt}; std::optional> occlusion_attention_area{std::nullopt}; - std::optional ego_lane{std::nullopt}; std::optional> adjacent_area{std::nullopt}; std::optional first_attention_area{std::nullopt}; std::optional second_attention_area{std::nullopt}; + std::optional ego_lane{std::nullopt}; std::optional stuck_vehicle_detect_area{std::nullopt}; std::optional> yield_stuck_detect_area{std::nullopt}; + std::optional candidate_collision_ego_lane_polygon{std::nullopt}; - std::vector candidate_collision_object_polygons; - autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; - autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; - autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects safe_under_traffic_control_targets; + autoware_auto_perception_msgs::msg::PredictedObjects unsafe_targets; + autoware_auto_perception_msgs::msg::PredictedObjects misjudge_targets; + autoware_auto_perception_msgs::msg::PredictedObjects too_late_detect_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; + autoware_auto_perception_msgs::msg::PredictedObjects parked_targets; std::vector occlusion_polygons; std::optional> nearest_occlusion_projection{std::nullopt}; - autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; - std::optional absence_traffic_light_creep_wall{std::nullopt}; std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; }; using TimeDistanceArray = std::vector>; /** - * @struct * @brief categorize traffic light priority */ enum class TrafficPrioritizedLevel { @@ -246,6 +226,41 @@ class IntersectionModule : public SceneModuleInterface }; /** @} */ + /** + * @brief + */ + struct PassJudgeStatus + { + //! true if ego is over the 1st pass judge line + const bool is_over_1st_pass_judge; + + //! true if second_attention_lane exists and ego is over the 2nd pass judge line + const std::optional is_over_2nd_pass_judge; + + //! true only when ego passed 1st pass judge line safely for the first time + const bool safely_passed_1st_judge_line; + + //! true only when ego passed 2nd pass judge line safely for the first time + const bool safely_passed_2nd_judge_line; + }; + + /** + * @brief + */ + struct CollisionStatus + { + enum BlameType { + BLAME_AT_FIRST_PASS_JUDGE, + BLAME_AT_SECOND_PASS_JUDGE, + }; + const bool collision_detected; + const intersection::CollisionInterval::LanePosition collision_position; + const std::vector>> + too_late_detect_objects; + const std::vector>> + misjudge_objects; + }; + IntersectionModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, @@ -325,6 +340,9 @@ class IntersectionModule : public SceneModuleInterface //! cache discretized occlusion detection lanelets std::optional> occlusion_attention_divisions_{ std::nullopt}; + + //! save the time when ego observed green traffic light before entering the intersection + std::optional initial_green_light_observed_time_{std::nullopt}; /** @}*/ private: @@ -340,17 +358,19 @@ class IntersectionModule : public SceneModuleInterface bool is_permanent_go_{false}; //! for checking if ego is over the pass judge lines because previously the situation was SAFE - intersection::DecisionResult prev_decision_result_{intersection::Indecisive{""}}; + intersection::DecisionResult prev_decision_result_{intersection::InternalError{""}}; //! flag if ego passed the 1st_pass_judge_line while peeking. If this is true, 1st_pass_judge_line //! is treated as the same position as occlusion_peeking_stopline bool passed_1st_judge_line_while_peeking_{false}; - //! save the time when ego passed the 1st/2nd_pass_judge_line with safe decision. If collision is - //! expected after these variables are non-null, then it is the fault of past perception failure - //! at these time. - std::optional safely_passed_1st_judge_line_time_{std::nullopt}; - std::optional safely_passed_2nd_judge_line_time_{std::nullopt}; + //! save the time and ego position when ego passed the 1st/2nd_pass_judge_line with safe + //! decision. If collision is expected after these variables are non-null, then it is the fault of + //! past perception failure at these time. + std::optional> + safely_passed_1st_judge_line_time_{std::nullopt}; + std::optional> + safely_passed_2nd_judge_line_time_{std::nullopt}; /** @}*/ private: @@ -363,6 +383,9 @@ class IntersectionModule : public SceneModuleInterface */ //! debouncing for stable SAFE decision StateMachine collision_state_machine_; + + //! container for storing safety status of objects on the attention area + intersection::ObjectInfoManager object_info_manager_; /** @} */ private: @@ -388,8 +411,6 @@ class IntersectionModule : public SceneModuleInterface StateMachine static_occlusion_timeout_state_machine_; /** @} */ - std::optional initial_green_light_observed_time_{std::nullopt}; - private: /** *********************************************************** @@ -464,13 +485,13 @@ class IntersectionModule : public SceneModuleInterface /** * @brief prepare basic data structure - * @return return IntersectionStopLines if all data is valid, otherwise Indecisive + * @return return IntersectionStopLines if all data is valid, otherwise InternalError * @note if successful, it is ensure that intersection_lanelets_, * intersection_lanelets.first_conflicting_lane are not null * * To simplify modifyPathVelocityDetail(), this function is used at first */ - intersection::Result prepareIntersectionData( + intersection::Result prepareIntersectionData( const bool is_prioritized, PathWithLaneId * path); /** @@ -518,16 +539,6 @@ class IntersectionModule : public SceneModuleInterface const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); /** @} */ -private: - /** - * @defgroup utility [fn] utility member function - * @{ - */ - void stoppedAtPositionForDuration( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t position, - const double duration, StateMachine * state_machine); - /** @} */ - private: /** *********************************************************** @@ -547,14 +558,6 @@ class IntersectionModule : public SceneModuleInterface TrafficPrioritizedLevel getTrafficPrioritizedLevel() const; /** @} */ -private: - /** - * @brief categorize target objects - */ - TargetObjects generateTargetObjects( - const intersection::IntersectionLanelets & intersection_lanelets, - const std::optional & intersection_area) const; - private: /** *********************************************************** @@ -598,14 +601,12 @@ class IntersectionModule : public SceneModuleInterface std::optional isYieldStuckStatus( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionStopLines & intersection_stoplines, - const TargetObjects & target_objects) const; + const intersection::IntersectionStopLines & intersection_stoplines) const; /** * @brief check yield stuck */ bool checkYieldStuckVehicleInIntersection( - const TargetObjects & target_objects, const intersection::InterpolatedPathInfo & interpolated_path_info, const lanelet::ConstLanelets & attention_lanelets) const; /** @} */ @@ -621,27 +622,21 @@ class IntersectionModule : public SceneModuleInterface /** * @brief check occlusion status * @attention this function has access to value() of occlusion_attention_divisions_, - * intersection_lanelets.first_attention_area() + * intersection_lanelets_ intersection_lanelets.first_attention_area() */ std::tuple< OcclusionType, bool /* module detection with margin */, bool /* reconciled occlusion disapproval */> getOcclusionStatus( const TrafficPrioritizedLevel & traffic_prioritized_level, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionLanelets & intersection_lanelets, - const TargetObjects & target_objects); + const intersection::InterpolatedPathInfo & interpolated_path_info); /** * @brief calculate detected occlusion status(NOT | STATICALLY | DYNAMICALLY) + * @attention this function has access to value() of intersection_lanelets_, + * intersection_lanelets.first_attention_area(), occlusion_attention_divisions_ */ - OcclusionType detectOcclusion( - const std::vector & attention_areas, - const lanelet::ConstLanelets & adjacent_lanelets, - const lanelet::CompoundPolygon3d & first_attention_area, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const TargetObjects & target_objects); + OcclusionType detectOcclusion(const intersection::InterpolatedPathInfo & interpolated_path_info); /** @} */ private: @@ -654,15 +649,12 @@ class IntersectionModule : public SceneModuleInterface */ /** * @brief check if ego is already over the pass judge line - * @return if ego is over both 1st/2nd pass judge lines, return Indecisive, else return + * @return if ego is over both 1st/2nd pass judge lines, return InternalError, else return * (is_over_1st_pass_judge, is_over_2nd_pass_judge) * @attention this function has access to value() of intersection_stoplines.default_stopline, * intersection_stoplines.occlusion_stopline */ - intersection::Result< - intersection::Indecisive, - std::pair> - isOverPassJudgeLinesStatus( + PassJudgeStatus isOverPassJudgeLinesStatus( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, const intersection::IntersectionStopLines & intersection_stoplines); /** @} */ @@ -678,6 +670,25 @@ class IntersectionModule : public SceneModuleInterface bool isTargetCollisionVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + /** + * @brief find the objects on attention_area/intersection_area and update positional information + * @attention this function has access to value() of intersection_lanelets_ + */ + void updateObjectInfoManagerArea(); + + /** + * @brief find the collision Interval/CollisionKnowledge of registered objects + * @attention this function has access to value() of intersection_lanelets_ + */ + void updateObjectInfoManagerCollision( + const intersection::PathLanelets & path_lanelets, const TimeDistanceArray & time_distance_array, + const TrafficPrioritizedLevel & traffic_prioritized_level, + const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time); + + void cutPredictPathWithinDuration( + const builtin_interfaces::msg::Time & object_stamp, const double time_thr, + autoware_auto_perception_msgs::msg::PredictedPath * predicted_path) const; + /** * @brief check if there are any objects around the stoplines on the attention areas when ego * entered the intersection on green light @@ -687,44 +698,58 @@ class IntersectionModule : public SceneModuleInterface * intersection_stoplines.occlusion_peeking_stopline */ std::optional isGreenPseudoCollisionStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const size_t collision_stopline_idx, - const intersection::IntersectionStopLines & intersection_stoplines, - const TargetObjects & target_objects); + const size_t closest_idx, const size_t collision_stopline_idx, + const intersection::IntersectionStopLines & intersection_stoplines); + + /** + * @brief generate the message explaining why too_late_detect_objects/misjudge_objects exist and + * blame past perception fault + */ + std::string generateDetectionBlameDiagnosis( + const std::vector< + std::pair>> & + too_late_detect_objects, + const std::vector< + std::pair>> & + misjudge_objects) const; /** - * @brief check collision + * @brief generate the message explaining how much ego should accelerate to avoid future dangerous + * situation */ - bool checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, - const intersection::PathLanelets & path_lanelets, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, - const TrafficPrioritizedLevel & traffic_prioritized_level); + std::string generateEgoRiskEvasiveDiagnosis( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const TimeDistanceArray & ego_time_distance_array, + const std::vector< + std::pair>> & + too_late_detect_objects, + const std::vector< + std::pair>> & + misjudge_objects) const; + + /** + * @brief return if collision is detected and the collision position + */ + CollisionStatus detectCollision( + const bool is_over_1st_pass_judge_line, const std::optional is_over_2nd_pass_judge_line); std::optional checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, const bool is_parked_vehicle) const; - void cutPredictPathWithDuration(TargetObjects * target_objects, const double time_thr); - /** * @brief calculate ego vehicle profile along the path inside the intersection as the sequence of * (time of arrival, traveled distance) from current ego position + * @attention this function has access to value() of + * intersection_stoplines.occlusion_peeking_stopline, + * intersection_stoplines.first_attention_stopline */ TimeDistanceArray calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, + const intersection::IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); /** @} */ - /* - bool IntersectionModule::checkFrontVehicleDeceleration( - lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, - const Polygon2d & stuck_vehicle_detect_area, - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const double assumed_front_car_decel); - */ - mutable DebugData debug_data_; rclcpp::Publisher::SharedPtr decision_state_pub_; rclcpp::Publisher::SharedPtr ego_ttc_pub_; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 43bb68cf56f67..4496df77134e2 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -18,42 +18,18 @@ #include // for toGeomPoly #include // for smoothPath #include +#include #include #include // for toPolygon2d #include -#include #include +#include +#include +#include #include -namespace -{ -tier4_autoware_utils::Polygon2d createOneStepPolygon( - const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, - const autoware_auto_perception_msgs::msg::Shape & shape) -{ - namespace bg = boost::geometry; - const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); - const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); - - tier4_autoware_utils::Polygon2d one_step_poly; - for (const auto & point : prev_poly.outer()) { - one_step_poly.outer().push_back(point); - } - for (const auto & point : next_poly.outer()) { - one_step_poly.outer().push_back(point); - } - - bg::correct(one_step_poly); - - tier4_autoware_utils::Polygon2d convex_one_step_poly; - bg::convex_hull(one_step_poly, convex_one_step_poly); - - return convex_one_step_poly; -} -} // namespace - namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -79,38 +55,303 @@ bool IntersectionModule::isTargetCollisionVehicleType( return false; } -std::optional -IntersectionModule::isGreenPseudoCollisionStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const size_t collision_stopline_idx, - const intersection::IntersectionStopLines & intersection_stoplines, - const TargetObjects & target_objects) +void IntersectionModule::updateObjectInfoManagerArea() { + const auto & intersection_lanelets = intersection_lanelets_.value(); + const auto & attention_lanelets = intersection_lanelets.attention(); + const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stoplines(); + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); - // If there are any vehicles on the attention area when ego entered the intersection on green - // light, do pseudo collision detection because the vehicles are very slow and no collisions may - // be detected. check if ego vehicle entered assigned lanelet - const bool is_green_solid_on = isGreenSolidOn(); - if (!is_green_solid_on) { - return std::nullopt; + const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); + + // ========================================================================================== + // entries that are not observed in this iteration need to be cleared + // + // NOTE: old_map is not referenced because internal data of object_info_manager is cleared + // ========================================================================================== + const auto old_map = object_info_manager_.getObjectsMap(); + object_info_manager_.clearObjects(); + + for (const auto & predicted_object : planner_data_->predicted_objects->objects) { + if (!isTargetCollisionVehicleType(predicted_object)) { + continue; + } + + // ========================================================================================== + // NOTE: is_parked_vehicle is used because sometimes slow vehicle direction is + // incorrect/reversed/flipped due to tracking. if is_parked_vehicle is true, object direction + // is not checked + // ========================================================================================== + const auto object_direction = + util::getObjectPoseWithVelocityDirection(predicted_object.kinematics); + const auto is_parked_vehicle = + std::fabs(predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x) < + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold; + + const auto belong_adjacent_lanelet_id = + checkAngleForTargetLanelets(object_direction, adjacent_lanelets, false); + if (belong_adjacent_lanelet_id) { + continue; + } + const auto belong_attention_lanelet_id = + checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle); + const auto & obj_pos = predicted_object.kinematics.initial_pose_with_covariance.pose.position; + const bool in_intersection_area = [&]() { + if (!intersection_area) { + return false; + } + return bg::within( + tier4_autoware_utils::Point2d{obj_pos.x, obj_pos.y}, intersection_area.value()); + }(); + std::optional attention_lanelet{std::nullopt}; + std::optional stopline{std::nullopt}; + if (!belong_attention_lanelet_id && !in_intersection_area) { + continue; + } else if (belong_attention_lanelet_id) { + const auto idx = belong_attention_lanelet_id.value(); + attention_lanelet = attention_lanelets.at(idx); + stopline = attention_lanelet_stoplines.at(idx); + } + + const auto object_it = old_map.find(predicted_object.object_id); + if (object_it != old_map.end()) { + auto object_info = object_it->second; + object_info_manager_.registerExistingObject( + predicted_object.object_id, belong_attention_lanelet_id.has_value(), in_intersection_area, + is_parked_vehicle, object_info); + object_info->initialize(predicted_object, attention_lanelet, stopline); + } else { + auto object_info = object_info_manager_.registerObject( + predicted_object.object_id, belong_attention_lanelet_id.has_value(), in_intersection_area, + is_parked_vehicle); + object_info->initialize(predicted_object, attention_lanelet, stopline); + } } - const auto closest_idx = intersection_stoplines.closest_idx; - const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); - if (!initial_green_light_observed_time_) { - const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); - const bool approached_assigned_lane = - motion_utils::calcSignedArcLength( - path.points, closest_idx, - tier4_autoware_utils::createPoint( - assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), - assigned_lane_begin_point.z())) < - planner_param_.collision_detection.yield_on_green_traffic_light - .distance_to_assigned_lanelet_start; - if (approached_assigned_lane) { - initial_green_light_observed_time_ = clock_->now(); +} + +void IntersectionModule::updateObjectInfoManagerCollision( + const intersection::PathLanelets & path_lanelets, + const IntersectionModule::TimeDistanceArray & time_distance_array, + const IntersectionModule::TrafficPrioritizedLevel & traffic_prioritized_level, + const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time) +{ + const auto & intersection_lanelets = intersection_lanelets_.value(); + + if (passed_1st_judge_line_first_time) { + object_info_manager_.setPassed1stPassJudgeLineFirstTime(clock_->now()); + } + if (passed_2nd_judge_line_first_time) { + object_info_manager_.setPassed2ndPassJudgeLineFirstTime(clock_->now()); + } + + const double passing_time = time_distance_array.back().first; + const auto & concat_lanelets = path_lanelets.all; + const auto closest_arc_coords = + lanelet::utils::getArcCoordinates(concat_lanelets, planner_data_->current_odometry->pose); + const auto & ego_lane = path_lanelets.ego_or_entry2exit; + debug_data_.ego_lane = ego_lane.polygon3d(); + const auto ego_poly = ego_lane.polygon2d().basicPolygon(); + + // ========================================================================================== + // dynamically change TTC margin according to traffic light color to gradually relax from green to + // red + // ========================================================================================== + const auto [collision_start_margin_time, collision_end_margin_time] = [&]() { + if (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED) { + return std::make_pair( + planner_param_.collision_detection.fully_prioritized.collision_start_margin_time, + planner_param_.collision_detection.fully_prioritized.collision_end_margin_time); + } + if (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) { + return std::make_pair( + planner_param_.collision_detection.partially_prioritized.collision_start_margin_time, + planner_param_.collision_detection.partially_prioritized.collision_end_margin_time); + } + return std::make_pair( + planner_param_.collision_detection.not_prioritized.collision_start_margin_time, + planner_param_.collision_detection.not_prioritized.collision_end_margin_time); + }(); + + for (auto & object_info : object_info_manager_.attentionObjects()) { + const auto & predicted_object = object_info->predicted_object(); + bool safe_under_traffic_control = false; + if ( + traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && + object_info->can_stop_before_stopline( + planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration)) { + safe_under_traffic_control = true; + } + if ( + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED && + object_info->can_stop_before_ego_lane( + planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration, + planner_param_.collision_detection.ignore_on_red_traffic_light.object_margin_to_path, + ego_lane)) { + safe_under_traffic_control = true; + } + + // ========================================================================================== + // check the PredictedPath in the ascending order of its confidence to save the safe/unsafe + // CollisionKnowledge for most probable path + // ========================================================================================== + std::list sorted_predicted_paths; + for (unsigned i = 0; i < predicted_object.kinematics.predicted_paths.size(); ++i) { + sorted_predicted_paths.push_back(&predicted_object.kinematics.predicted_paths.at(i)); + } + sorted_predicted_paths.sort( + [](const auto path1, const auto path2) { return path1->confidence > path2->confidence; }); + + // ========================================================================================== + // if all of the predicted path is lower confidence/geometrically does not intersect with ego + // path, both will be null, which is interpreted as SAFE. if any of the path is "normal", either + // of them has value, not both + // ========================================================================================== + std::optional unsafe_interval{std::nullopt}; + std::optional safe_interval{std::nullopt}; + + for (const auto & predicted_path_ptr : sorted_predicted_paths) { + auto predicted_path = *predicted_path_ptr; + if ( + predicted_path.confidence < + planner_param_.collision_detection.min_predicted_path_confidence) { + continue; + } + cutPredictPathWithinDuration( + planner_data_->predicted_objects->header.stamp, passing_time, &predicted_path); + const auto object_passage_interval_opt = intersection::findPassageInterval( + predicted_path, predicted_object.shape, ego_poly, + intersection_lanelets.first_attention_lane(), + intersection_lanelets.second_attention_lane()); + if (!object_passage_interval_opt) { + // there is no chance of geometric collision for the entire prediction horizon + continue; + } + const auto & object_passage_interval = object_passage_interval_opt.value(); + const auto [object_enter_time, object_exit_time] = object_passage_interval.interval_time; + const auto ego_start_itr = std::lower_bound( + time_distance_array.begin(), time_distance_array.end(), + object_enter_time - collision_start_margin_time, + [](const auto & a, const double b) { return a.first < b; }); + if (ego_start_itr == time_distance_array.end()) { + // ========================================================================================== + // this is the case where at time "object_enter_time - collision_start_margin_time", ego is + // arriving at the exit of the intersection, which means even if we assume that the object + // accelerates and the first collision happens faster by the TTC margin, ego will be already + // arriving at the exist of the intersection. + // ========================================================================================== + continue; + } + auto ego_end_itr = std::lower_bound( + time_distance_array.begin(), time_distance_array.end(), + object_exit_time + collision_end_margin_time, + [](const auto & a, const double b) { return a.first < b; }); + if (ego_end_itr == time_distance_array.end()) { + ego_end_itr = time_distance_array.end() - 1; + } + const double ego_start_arc_length = std::max( + 0.0, closest_arc_coords.length + ego_start_itr->second - + planner_data_->vehicle_info_.rear_overhang_m); + const double ego_end_arc_length = std::min( + closest_arc_coords.length + ego_end_itr->second + + planner_data_->vehicle_info_.max_longitudinal_offset_m, + lanelet::utils::getLaneletLength2d(concat_lanelets)); + const auto trimmed_ego_polygon = lanelet::utils::getPolygonFromArcLength( + concat_lanelets, ego_start_arc_length, ego_end_arc_length); + if (trimmed_ego_polygon.empty()) { + continue; + } + Polygon2d polygon{}; + for (const auto & p : trimmed_ego_polygon) { + polygon.outer().emplace_back(p.x(), p.y()); + } + bg::correct(polygon); + debug_data_.candidate_collision_ego_lane_polygon = toGeomPoly(polygon); + + const auto & object_path = object_passage_interval.path; + const auto [begin, end] = object_passage_interval.interval_position; + bool collision_detected = false; + for (auto i = begin; i <= end; ++i) { + if (bg::intersects( + polygon, + tier4_autoware_utils::toPolygon2d(object_path.at(i), predicted_object.shape))) { + collision_detected = true; + break; + } + } + if (collision_detected) { + // if judged as UNSAFE, return + safe_interval = std::nullopt; + unsafe_interval = object_passage_interval; + break; + } + if (!safe_interval) { + // ========================================================================================== + // save the safe_decision_knowledge for the most probable path. this value is nullified if + // judged UNSAFE during the iteration + // ========================================================================================== + safe_interval = object_passage_interval; + } + } + object_info->update_safety(unsafe_interval, safe_interval, safe_under_traffic_control); + if (passed_1st_judge_line_first_time) { + object_info->setDecisionAt1stPassJudgeLinePassage(intersection::CollisionKnowledge{ + clock_->now(), // stamp + unsafe_interval + ? intersection::CollisionKnowledge::SafeType::UNSAFE + : (safe_under_traffic_control + ? intersection::CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL + : intersection::CollisionKnowledge::SafeType::SAFE), // safe + unsafe_interval ? unsafe_interval : safe_interval, // interval + predicted_object.kinematics.initial_twist_with_covariance.twist.linear + .x // observed_velocity + }); + } + if (passed_2nd_judge_line_first_time) { + object_info->setDecisionAt2ndPassJudgeLinePassage(intersection::CollisionKnowledge{ + clock_->now(), // stamp + unsafe_interval + ? intersection::CollisionKnowledge::SafeType::UNSAFE + : (safe_under_traffic_control + ? intersection::CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL + : intersection::CollisionKnowledge::SafeType::SAFE), // safe + unsafe_interval ? unsafe_interval : safe_interval, // interval + predicted_object.kinematics.initial_twist_with_covariance.twist.linear + .x // observed_velocity + }); + } + } +} + +void IntersectionModule::cutPredictPathWithinDuration( + const builtin_interfaces::msg::Time & object_stamp, const double time_thr, + autoware_auto_perception_msgs::msg::PredictedPath * path) const +{ + const rclcpp::Time current_time = clock_->now(); + const auto original_path = path->path; + path->path.clear(); + + for (size_t k = 0; k < original_path.size(); ++k) { // each path points + const auto & predicted_pose = original_path.at(k); + const auto predicted_time = + rclcpp::Time(object_stamp) + rclcpp::Duration(path->time_step) * static_cast(k); + if ((predicted_time - current_time).seconds() < time_thr) { + path->path.push_back(predicted_pose); } } +} + +std::optional +IntersectionModule::isGreenPseudoCollisionStatus( + const size_t closest_idx, const size_t collision_stopline_idx, + const intersection::IntersectionStopLines & intersection_stoplines) +{ + // ========================================================================================== + // if there are any vehicles on the attention area when ego entered the intersection on green + // light, do pseudo collision detection because collision is likely to happen. + // ========================================================================================== if (initial_green_light_observed_time_) { const auto now = clock_->now(); const bool still_wait = @@ -119,22 +360,344 @@ IntersectionModule::isGreenPseudoCollisionStatus( if (!still_wait) { return std::nullopt; } + const auto & attention_objects = object_info_manager_.attentionObjects(); const bool exist_close_vehicles = std::any_of( - target_objects.all_attention_objects.begin(), target_objects.all_attention_objects.end(), - [&](const auto & object) { - return object.dist_to_stopline.has_value() && - object.dist_to_stopline.value() < - planner_param_.collision_detection.yield_on_green_traffic_light - .object_dist_to_stopline; + attention_objects.begin(), attention_objects.end(), [&](const auto & object_info) { + return object_info->before_stopline_by( + planner_param_.collision_detection.yield_on_green_traffic_light.object_dist_to_stopline); }); if (exist_close_vehicles) { + const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); return intersection::NonOccludedCollisionStop{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + closest_idx, collision_stopline_idx, occlusion_stopline_idx, std::string("")}; } } return std::nullopt; } +std::string IntersectionModule::generateDetectionBlameDiagnosis( + const std::vector>> & + too_late_detect_objects, + const std::vector>> & + misjudge_objects) const +{ + std::string diag; + if (!safely_passed_1st_judge_line_time_) { + return diag; + } + const auto [passed_1st_judge_line_time, passed_1st_judge_line_pose] = + safely_passed_1st_judge_line_time_.value(); + const auto passed_1st_judge_line_time_double = + static_cast(passed_1st_judge_line_time.nanoseconds()) / 1e+9; + + const auto now = clock_->now(); + const auto now_double = static_cast(now.nanoseconds()) / 1e+9; + + // CAVEAT: format library causes runtime fault if the # of placeholders is more than the # of + // vargs + for (const auto & [blame_type, object_info] : too_late_detect_objects) { + if ( + blame_type == CollisionStatus::BLAME_AT_FIRST_PASS_JUDGE && object_info->unsafe_interval()) { + const auto & unsafe_interval = object_info->unsafe_interval().value(); + const double time_diff = now_double - passed_1st_judge_line_time_double; + diag += fmt::format( + "object {0} was not detected when ego passed the 1st pass judge line at {1}, but now at " + "{2}, collision is detected after {3}~{4} seconds on the first attention lanelet of type " + "{5}.\n", + object_info->uuid_str, // 0 + passed_1st_judge_line_time_double, // 1 + now_double, // 2 + unsafe_interval.interval_time.first, // 3 + unsafe_interval.interval_time.second, // 4 + magic_enum::enum_name(unsafe_interval.lane_position) // 5 + ); + const auto past_position_opt = object_info->estimated_past_position(time_diff); + if (past_position_opt) { + const auto & past_position = past_position_opt.value(); + diag += fmt::format( + "this object is estimated to have been at x = {0}, y = {1} when ego passed the 1st pass " + "judge line({2} seconds before from now) given the estimated current velocity {3}[m/s]. " + "ego was at x = {4}, y = {5} when it passed the 1st pass judge line so it is the fault " + "of detection side that failed to detect around {6}[m] range at that time.\n", + past_position.x, // 0 + past_position.y, // 1 + time_diff, // 2 + object_info->observed_velocity(), // 3 + passed_1st_judge_line_pose.position.x, // 4 + passed_1st_judge_line_pose.position.y, // 5 + tier4_autoware_utils::calcDistance2d(passed_1st_judge_line_pose, past_position) // 6 + ); + } + } + if ( + safely_passed_2nd_judge_line_time_ && + blame_type == CollisionStatus::BLAME_AT_SECOND_PASS_JUDGE && object_info->unsafe_interval()) { + const auto [passed_2nd_judge_line_time, passed_2nd_judge_line_pose] = + safely_passed_2nd_judge_line_time_.value(); + const auto passed_2nd_judge_line_time_double = + static_cast(passed_2nd_judge_line_time.nanoseconds()) / 1e+9; + + const auto & unsafe_interval = object_info->unsafe_interval().value(); + const double time_diff = now_double - passed_2nd_judge_line_time_double; + diag += fmt::format( + "object {0} was not detected when ego passed the 2nd pass judge line at {1}, but now at " + "{2}, collision is detected after {3}~{4} seconds on the lanelet of type {5}.\n", + object_info->uuid_str, // 0 + passed_2nd_judge_line_time_double, // 1 + now_double, // 2 + unsafe_interval.interval_time.first, // 3 + unsafe_interval.interval_time.second, // 4 + magic_enum::enum_name(unsafe_interval.lane_position) // 5 + ); + const auto past_position_opt = object_info->estimated_past_position(time_diff); + if (past_position_opt) { + const auto & past_position = past_position_opt.value(); + diag += fmt::format( + "this object is estimated to have been at x = {0}, y = {1} when ego passed the 2nd pass " + "judge line({2} seconds before from now) given the estimated current velocity {3}[m/s]. " + "ego was at x = {4}, y = {5} when it passed the 2nd pass judge line so it is the fault " + "of detection side that failed to detect around {6}[m] range at that time.\n", + past_position.x, // 0 + past_position.y, // 1 + time_diff, // 2 + object_info->observed_velocity(), // 3 + passed_2nd_judge_line_pose.position.x, // 4 + passed_2nd_judge_line_pose.position.y, // 5 + tier4_autoware_utils::calcDistance2d(passed_2nd_judge_line_pose, past_position) // 6 + ); + } + } + } + for (const auto & [blame_type, object_info] : misjudge_objects) { + if ( + blame_type == CollisionStatus::BLAME_AT_FIRST_PASS_JUDGE && object_info->unsafe_interval() && + object_info->decision_at_1st_pass_judge_line_passage()) { + const auto & decision_at_1st_pass_judge_line = + object_info->decision_at_1st_pass_judge_line_passage().value(); + const auto decision_at_1st_pass_judge_line_time = + static_cast(decision_at_1st_pass_judge_line.stamp.nanoseconds()) / 1e+9; + const auto & unsafe_interval = object_info->unsafe_interval().value(); + diag += fmt::format( + "object {0} was judged as {1} when ego passed the 1st pass judge line at time {2} " + "previously with the estimated velocity {3}[m/s], but now at {4} collision is detected " + "after {5}~{6} seconds on the first attention lanelet of type {7} with the estimated " + "current velocity {8}[m/s]\n", + object_info->uuid_str, // 0 + magic_enum::enum_name(decision_at_1st_pass_judge_line.safe_type), // 1 + decision_at_1st_pass_judge_line_time, // 2 + decision_at_1st_pass_judge_line.observed_velocity, // 3 + now_double, // 4 + unsafe_interval.interval_time.first, // 5 + unsafe_interval.interval_time.second, // 6 + magic_enum::enum_name(unsafe_interval.lane_position), // 7 + object_info->observed_velocity() // 8 + ); + } + if ( + blame_type == CollisionStatus::BLAME_AT_SECOND_PASS_JUDGE && object_info->unsafe_interval() && + object_info->decision_at_2nd_pass_judge_line_passage()) { + const auto & decision_at_2nd_pass_judge_line = + object_info->decision_at_2nd_pass_judge_line_passage().value(); + const auto decision_at_2nd_pass_judge_line_time = + static_cast(decision_at_2nd_pass_judge_line.stamp.nanoseconds()) / 1e+9; + const auto & unsafe_interval = object_info->unsafe_interval().value(); + diag += fmt::format( + "object {0} was judged as {1} when ego passed the 2nd pass judge line at time {2} " + "previously with the estimated velocity {3}[m/s], but now at {4} collision is detected " + "after {5}~{6} seconds on the lanelet of type {7} with the estimated current velocity " + "{8}[m/s]\n", + object_info->uuid_str, // 0 + magic_enum::enum_name(decision_at_2nd_pass_judge_line.safe_type), // 1 + decision_at_2nd_pass_judge_line_time, // 2 + decision_at_2nd_pass_judge_line.observed_velocity, // 3 + now_double, // 4 + unsafe_interval.interval_time.first, // 5 + unsafe_interval.interval_time.second, // 6 + magic_enum::enum_name(unsafe_interval.lane_position), // 7 + object_info->observed_velocity() // 8 + ); + } + } + return diag; +} + +std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const IntersectionModule::TimeDistanceArray & ego_time_distance_array, + const std::vector>> & + too_late_detect_objects, + [[maybe_unused]] const std::vector>> & + misjudge_objects) const +{ + static constexpr double min_vel = 1e-2; + std::string diag; + const double ego_vel = planner_data_->current_velocity->twist.linear.x; + for (const auto & [blame_type, object_info] : too_late_detect_objects) { + if (!object_info->unsafe_interval()) { + continue; + } + // object side + const auto & unsafe_interval = object_info->unsafe_interval().value(); + const auto [begin, end] = unsafe_interval.interval_position; + const auto &p1 = unsafe_interval.path.at(begin).position, + p2 = unsafe_interval.path.at(end).position; + const auto collision_pos = + tier4_autoware_utils::createPoint((p1.x + p2.x) / 2, (p1.y + p2.y) / 2, (p1.z + p2.z) / 2); + const auto object_dist_to_margin_point = + tier4_autoware_utils::calcDistance2d( + object_info->predicted_object().kinematics.initial_pose_with_covariance.pose.position, + collision_pos) - + planner_param_.collision_detection.avoid_collision_by_acceleration + .object_time_margin_to_collision_point * + object_info->observed_velocity(); + if (object_dist_to_margin_point <= 0.0) { + continue; + } + const auto object_eta_to_margin_point = + object_dist_to_margin_point / std::max(min_vel, object_info->observed_velocity()); + // ego side + const double ego_dist_to_collision_pos = + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_pos); + const auto ego_eta_to_collision_pos_it = std::lower_bound( + ego_time_distance_array.begin(), ego_time_distance_array.end(), ego_dist_to_collision_pos, + [](const auto & a, const double b) { return a.second < b; }); + if (ego_eta_to_collision_pos_it == ego_time_distance_array.end()) { + continue; + } + const double ego_eta_to_collision_pos = ego_eta_to_collision_pos_it->first; + if (ego_eta_to_collision_pos < object_eta_to_margin_point) { + // this case, ego will pass the collision point before this object get close to the collision + // point within margin just by keeping current plan, so no need to accelerate + continue; + } + diag += fmt::format( + "the object is expected to approach the collision point by the TTC margin in {0} seconds, " + "while ego will arrive there in {1} seconds, so ego needs to accelerate from current " + "velocity {2}[m/s] to {3}[m/s]\n", + object_eta_to_margin_point, // 0 + ego_eta_to_collision_pos, // 1 + ego_vel, // 2 + ego_dist_to_collision_pos / object_eta_to_margin_point // 3 + ); + } + return diag; +} + +IntersectionModule::CollisionStatus IntersectionModule::detectCollision( + const bool is_over_1st_pass_judge_line, const std::optional is_over_2nd_pass_judge_line) +{ + // ========================================================================================== + // if collision is detected for multiple objects, we prioritize collision on the first + // attention lanelet + // ========================================================================================== + bool collision_at_first_lane = false; + bool collision_at_non_first_lane = false; + + // ========================================================================================== + // find the objects which is judges as UNSAFE after ego passed pass judge lines. + // + // misjudge_objects are those that were once judged as safe when ego passed the pass judge line + // + // too_late_detect objects are those that (1) were not detected when ego passed the pass judge + // line (2) were judged as dangerous at the same time when ego passed the pass judge, which are + // expected to have been detected in the prior iteration because ego could have judged as UNSAFE + // in the prior iteration + // ========================================================================================== + std::vector>> + misjudge_objects; + std::vector>> + too_late_detect_objects; + for (const auto & object_info : object_info_manager_.attentionObjects()) { + if (object_info->is_safe_under_traffic_control()) { + debug_data_.safe_under_traffic_control_targets.objects.push_back( + object_info->predicted_object()); + continue; + } + if (!object_info->is_unsafe()) { + continue; + } + const auto & unsafe_info = object_info->is_unsafe().value(); + setObjectsOfInterestData( + object_info->predicted_object().kinematics.initial_pose_with_covariance.pose, + object_info->predicted_object().shape, ColorName::RED); + // ========================================================================================== + // if ego is over the pass judge lines, then the visualization as "too_late_objects" or + // "misjudge_objects" is more important than that for "unsafe" + // + // NOTE: consider a vehicle which was not detected at 1st_pass_judge_passage, and now collision + // detected on the 1st lane, which is "too_late" for 1st lane passage, but once it decelerated + // or yielded, so it turned safe, and ego passed the 2nd pass judge line, but at the same it + // accelerated again, which is "misjudge" for 2nd lane passage. In this case this vehicle is + // visualized as "misjudge" + // ========================================================================================== + auto * debug_container = &debug_data_.unsafe_targets.objects; + if (unsafe_info.lane_position == intersection::CollisionInterval::LanePosition::FIRST) { + collision_at_first_lane = true; + } else { + collision_at_non_first_lane = true; + } + if ( + is_over_1st_pass_judge_line && + unsafe_info.lane_position == intersection::CollisionInterval::LanePosition::FIRST) { + const auto & decision_at_1st_pass_judge_opt = + object_info->decision_at_1st_pass_judge_line_passage(); + if (!decision_at_1st_pass_judge_opt) { + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info); + debug_container = &debug_data_.too_late_detect_targets.objects; + } else { + const auto & decision_at_1st_pass_judge = decision_at_1st_pass_judge_opt.value(); + if ( + decision_at_1st_pass_judge.safe_type != + intersection::CollisionKnowledge::SafeType::UNSAFE) { + misjudge_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info); + debug_container = &debug_data_.misjudge_targets.objects; + } else { + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info); + debug_container = &debug_data_.too_late_detect_targets.objects; + } + } + } + if (is_over_2nd_pass_judge_line && is_over_2nd_pass_judge_line.value()) { + const auto & decision_at_2nd_pass_judge_opt = + object_info->decision_at_2nd_pass_judge_line_passage(); + if (!decision_at_2nd_pass_judge_opt) { + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info); + debug_container = &debug_data_.too_late_detect_targets.objects; + } else { + const auto & decision_at_2nd_pass_judge = decision_at_2nd_pass_judge_opt.value(); + if ( + decision_at_2nd_pass_judge.safe_type != + intersection::CollisionKnowledge::SafeType::UNSAFE) { + misjudge_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info); + debug_container = &debug_data_.misjudge_targets.objects; + } else { + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info); + debug_container = &debug_data_.too_late_detect_targets.objects; + } + } + } + debug_container->emplace_back(object_info->predicted_object()); + } + if (collision_at_first_lane) { + return { + true, intersection::CollisionInterval::FIRST, too_late_detect_objects, misjudge_objects}; + } else if (collision_at_non_first_lane) { + return {true, intersection::CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; + } + return {false, intersection::CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; +} + +/* bool IntersectionModule::checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, const intersection::PathLanelets & path_lanelets, const size_t closest_idx, @@ -350,7 +913,7 @@ bool IntersectionModule::checkCollision( } } object_ttc_time_array.layout.dim.at(0).size++; - const auto & pos = object.kinematics.initial_pose_with_covariance.pose.position; + const auto & pos = object..position; const auto & shape = object.shape; object_ttc_time_array.data.insert( object_ttc_time_array.data.end(), @@ -383,6 +946,7 @@ bool IntersectionModule::checkCollision( return collision_detected; } +*/ std::optional IntersectionModule::checkAngleForTargetLanelets( @@ -422,32 +986,9 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( return std::nullopt; } -void IntersectionModule::cutPredictPathWithDuration( - TargetObjects * target_objects, const double time_thr) -{ - const rclcpp::Time current_time = clock_->now(); - for (auto & target_object : target_objects->all_attention_objects) { // each objects - for (auto & predicted_path : - target_object.object.kinematics.predicted_paths) { // each predicted paths - const auto origin_path = predicted_path; - predicted_path.path.clear(); - - for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points - const auto & predicted_pose = origin_path.path.at(k); - const auto predicted_time = - rclcpp::Time(target_objects->header.stamp) + - rclcpp::Duration(origin_path.time_step) * static_cast(k); - if ((predicted_time - current_time).seconds() < time_thr) { - predicted_path.path.push_back(predicted_pose); - } - } - } - } -} - IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, + const intersection::IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) { const double intersection_velocity = @@ -460,8 +1001,42 @@ IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassin planner_param_.collision_detection.velocity_profile.minimum_upstream_velocity; const double current_velocity = planner_data_->current_velocity->twist.linear.x; - int assigned_lane_found = false; + // ========================================================================================== + // if ego is waiting for collision detection, the entry time into the intersection + // is a bit delayed for the chattering hold, so we need to "shift" the TimeDistanceArray by + // this delay + // ========================================================================================== + const bool is_go_out = (activated_ && occlusion_activated_); + const double time_delay = (is_go_out || is_prioritized) + ? 0.0 + : (planner_param_.collision_detection.collision_detection_hold_time - + collision_state_machine_.getDuration()); + // ========================================================================================== + // to account for the stopline generated by upstream behavior_velocity modules (like walkway, + // crosswalk), if use_upstream flag is true, the raw velocity of path points after + // last_intersection_stopline_candidate_idx is used, which maybe almost-zero. at those almost-zero + // velocity path points, ego future profile is almost "fixed" there. + // + // last_intersection_stopline_candidate_idx must be carefully decided especially when ego + // velocity is almost zero, because if last_intersection_stopline_candidate_idx is at the + // closest_idx for example, ego is almost "fixed" at current position for the entire + // spatiotemporal profile, which is judged as SAFE because that profile does not collide + // with the predicted paths of objects. + // + // if second_attention_lane exists, second_attention_stopline_idx is used. if not, + // max(occlusion_stopline_idx, first_attention_stopline_idx) is used because + // occlusion_stopline_idx varies depending on the peeking offset parameter + // ========================================================================================== + const auto second_attention_stopline_idx = intersection_stoplines.second_attention_stopline; + const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); + const auto first_attention_stopline_idx = intersection_stoplines.first_attention_stopline.value(); + const auto closest_idx = intersection_stoplines.closest_idx; + const auto last_intersection_stopline_candidate_idx = + second_attention_stopline_idx ? second_attention_stopline_idx.value() + : std::max(occlusion_stopline_idx, first_attention_stopline_idx); + + int assigned_lane_found = false; // crop intersection part of the path, and set the reference velocity to intersection_velocity // for ego's ttc PathWithLaneId reference_path; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp index 353826070eff7..b7c2d4c12878c 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -34,25 +34,18 @@ std::tuple< bool /* reconciled occlusion disapproval */> IntersectionModule::getOcclusionStatus( const TrafficPrioritizedLevel & traffic_prioritized_level, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionLanelets & intersection_lanelets, - const TargetObjects & target_objects) + const intersection::InterpolatedPathInfo & interpolated_path_info) { - const auto & adjacent_lanelets = intersection_lanelets.adjacent(); + const auto & intersection_lanelets = intersection_lanelets_.value(); const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); - const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); - const auto first_attention_area = intersection_lanelets.first_attention_area().value(); const bool is_amber_or_red = (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED); // check occlusion on detection lane - const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); auto occlusion_status = (planner_param_.occlusion.enable && !occlusion_attention_lanelets.empty() && !is_amber_or_red) - ? detectOcclusion( - occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, - occlusion_attention_divisions, target_objects) + ? detectOcclusion(interpolated_path_info) : OcclusionType::NOT_OCCLUDED; occlusion_stop_state_machine_.setStateWithMarginTime( occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, @@ -76,13 +69,14 @@ IntersectionModule::getOcclusionStatus( } IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( - const std::vector & attention_areas, - const lanelet::ConstLanelets & adjacent_lanelets, - const lanelet::CompoundPolygon3d & first_attention_area, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const TargetObjects & target_objects) + const intersection::InterpolatedPathInfo & interpolated_path_info) { + const auto & intersection_lanelets = intersection_lanelets_.value(); + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); + const auto & attention_areas = intersection_lanelets.occlusion_attention_area(); + const auto first_attention_area = intersection_lanelets.first_attention_area().value(); + const auto & lane_divisions = occlusion_attention_divisions_.value(); + const auto & occ_grid = *planner_data_->occupancy_grid; const auto & current_pose = planner_data_->current_odometry->pose; const double occlusion_dist_thr = planner_param_.occlusion.occlusion_required_clearance_distance; @@ -208,13 +202,15 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( // re-use attention_mask attention_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); // (3.1) draw all cells on attention_mask behind blocking vehicles as not occluded - const auto & blocking_attention_objects = target_objects.parked_attention_objects; - for (const auto & blocking_attention_object : blocking_attention_objects) { - debug_data_.blocking_attention_objects.objects.push_back(blocking_attention_object.object); + const auto & blocking_attention_objects = object_info_manager_.parkedObjects(); + for (const auto & blocking_attention_object_info : blocking_attention_objects) { + debug_data_.parked_targets.objects.push_back( + blocking_attention_object_info->predicted_object()); } std::vector> blocking_polygons; - for (const auto & blocking_attention_object : blocking_attention_objects) { - const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object.object); + for (const auto & blocking_attention_object_info : blocking_attention_objects) { + const Polygon2d obj_poly = + tier4_autoware_utils::toPolygon2d(blocking_attention_object_info->predicted_object()); findCommonCvPolygons(obj_poly.outer(), blocking_polygons); } for (const auto & blocking_polygon : blocking_polygons) { @@ -390,14 +386,9 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( LineString2d ego_occlusion_line; ego_occlusion_line.emplace_back(current_pose.position.x, current_pose.position.y); ego_occlusion_line.emplace_back(nearest_occlusion_point.point.x, nearest_occlusion_point.point.y); - for (const auto & attention_object : target_objects.all_attention_objects) { - const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); - if (bg::intersects(obj_poly, ego_occlusion_line)) { - return OcclusionType::DYNAMICALLY_OCCLUDED; - } - } - for (const auto & attention_object : target_objects.intersection_area_objects) { - const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); + for (const auto & attention_object_info : object_info_manager_.allObjects()) { + const auto obj_poly = + tier4_autoware_utils::toPolygon2d(attention_object_info->predicted_object()); if (bg::intersects(obj_poly, ego_occlusion_line)) { return OcclusionType::DYNAMICALLY_OCCLUDED; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 746e615d8c6b0..c44008db9b08b 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -161,11 +161,13 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; -intersection::Result +using intersection::make_err; +using intersection::make_ok; +using intersection::Result; + +Result IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithLaneId * path) { - using intersection::Result; - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); @@ -177,26 +179,32 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL const auto interpolated_path_info_opt = util::generateInterpolatedPath( lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); if (!interpolated_path_info_opt) { - return Result::make_err( - intersection::Indecisive{"splineInterpolate failed"}); + return make_err( + "splineInterpolate failed"); } const auto & interpolated_path_info = interpolated_path_info_opt.value(); if (!interpolated_path_info.lane_id_interval) { - return Result::make_err( - intersection::Indecisive{ - "Path has no interval on intersection lane " + std::to_string(lane_id_)}); + return make_err( + "Path has no interval on intersection lane " + std::to_string(lane_id_)); } - // cache intersection lane information because it is invariant if (!intersection_lanelets_) { intersection_lanelets_ = generateObjectiveLanelets(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet); } auto & intersection_lanelets = intersection_lanelets_.value(); - - // at the very first time of regisTration of this module, the path may not be conflicting with the - // attention area, so update() is called to update the internal data as well as traffic light info + debug_data_.attention_area = intersection_lanelets.attention_area(); + debug_data_.first_attention_area = intersection_lanelets.first_attention_area(); + debug_data_.second_attention_area = intersection_lanelets.second_attention_area(); + debug_data_.occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); + debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); + + // ========================================================================================== + // at the very first time of registration of this module, the path may not be conflicting with + // the attention area, so update() is called to update the internal data as well as traffic + // light info + // ========================================================================================== intersection_lanelets.update( is_prioritized, interpolated_path_info, footprint, baselink2front, routing_graph_ptr); @@ -205,17 +213,17 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL const auto & first_conflicting_lane_opt = intersection_lanelets.first_conflicting_lane(); if (conflicting_lanelets.empty() || !first_conflicting_area_opt || !first_conflicting_lane_opt) { // this is abnormal - return Result::make_err( - intersection::Indecisive{"conflicting area is empty"}); + return make_err( + "conflicting area is empty"); } const auto & first_conflicting_lane = first_conflicting_lane_opt.value(); const auto & first_conflicting_area = first_conflicting_area_opt.value(); const auto & second_attention_area_opt = intersection_lanelets.second_attention_area(); - // generate all stop line candidates - // see the doc for struct IntersectionStopLines - /// even if the attention area is null, stuck vehicle stop line needs to be generated from - /// conflicting lanes + // ========================================================================================== + // even if the attention area is null, stuck vehicle stop line needs to be generated from + // conflicting lanes, so dummy_first_attention_lane is used + // ========================================================================================== const auto & dummy_first_attention_lane = intersection_lanelets.first_attention_lane() ? intersection_lanelets.first_attention_lane().value() : first_conflicting_lane; @@ -224,8 +232,8 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, second_attention_area_opt, interpolated_path_info, path); if (!intersection_stoplines_opt) { - return Result::make_err( - intersection::Indecisive{"failed to generate intersection_stoplines"}); + return make_err( + "failed to generate intersection_stoplines"); } const auto & intersection_stoplines = intersection_stoplines_opt.value(); const auto closest_idx = intersection_stoplines.closest_idx; @@ -239,8 +247,8 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL lanelets_on_path, interpolated_path_info, first_conflicting_area, conflicting_area, first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx); if (!path_lanelets_opt.has_value()) { - return Result::make_err( - intersection::Indecisive{"failed to generate PathLanelets"}); + return make_err( + "failed to generate PathLanelets"); } const auto & path_lanelets = path_lanelets_opt.value(); @@ -250,8 +258,24 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL planner_data_->occupancy_grid->info.resolution); } - return Result::make_ok( - BasicData{interpolated_path_info, intersection_stoplines, path_lanelets}); + const bool is_green_solid_on = isGreenSolidOn(); + if (is_green_solid_on && !initial_green_light_observed_time_) { + const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); + const bool approached_assigned_lane = + motion_utils::calcSignedArcLength( + path->points, closest_idx, + tier4_autoware_utils::createPoint( + assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), + assigned_lane_begin_point.z())) < + planner_param_.collision_detection.yield_on_green_traffic_light + .distance_to_assigned_lanelet_start; + if (approached_assigned_lane) { + initial_green_light_observed_time_ = clock_->now(); + } + } + + return make_ok( + interpolated_path_info, intersection_stoplines, path_lanelets); } std::optional IntersectionModule::getStopLineIndexFromMap( @@ -421,8 +445,10 @@ IntersectionModule::generateIntersectionStopLines( int stuck_stopline_ip_int = 0; bool stuck_stopline_valid = true; if (use_stuck_stopline) { + // ========================================================================================== // NOTE: when ego vehicle is approaching attention area and already passed // first_conflicting_area, this could be null. + // ========================================================================================== const auto stuck_stopline_idx_ip_opt = util::getFirstPointInsidePolygonByFootprint( first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); if (!stuck_stopline_idx_ip_opt) { @@ -457,12 +483,13 @@ IntersectionModule::generateIntersectionStopLines( second_attention_stopline_ip_int >= 0 ? static_cast(second_attention_stopline_ip_int) : 0; - // (8) second pass judge line position on interpolated path. It is the same as first pass judge - // line if second_attention_lane is null - int second_pass_judge_ip_int = occlusion_wo_tl_pass_judge_line_ip; - const auto second_pass_judge_line_ip = - second_attention_area_opt ? static_cast(std::max(second_pass_judge_ip_int, 0)) - : first_pass_judge_line_ip; + // (8) second pass judge line position on interpolated path. It is null if second_attention_lane + // is null + size_t second_pass_judge_line_ip = occlusion_wo_tl_pass_judge_line_ip; + bool second_pass_judge_line_valid = false; + if (second_attention_area_opt) { + second_pass_judge_line_valid = true; + } struct IntersectionStopLinesTemp { @@ -528,9 +555,11 @@ IntersectionModule::generateIntersectionStopLines( intersection_stoplines.occlusion_peeking_stopline = intersection_stoplines_temp.occlusion_peeking_stopline; } + if (second_pass_judge_line_valid) { + intersection_stoplines.second_pass_judge_line = + intersection_stoplines_temp.second_pass_judge_line; + } intersection_stoplines.first_pass_judge_line = intersection_stoplines_temp.first_pass_judge_line; - intersection_stoplines.second_pass_judge_line = - intersection_stoplines_temp.second_pass_judge_line; intersection_stoplines.occlusion_wo_tl_pass_judge_line = intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; return intersection_stoplines; @@ -556,7 +585,7 @@ intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets } // for low priority lane - // If ego_lane has right of way (i.e. is high priority), + // if ego_lane has right of way (i.e. is high priority), // ignore yieldLanelets (i.e. low priority lanes) lanelet::ConstLanelets yield_lanelets{}; const auto right_of_ways = assigned_lanelet.regulatoryElementsAs(); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index 7f23bed3c36cd..389c73d651f1e 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -262,21 +262,19 @@ bool IntersectionModule::checkStuckVehicleInIntersection( std::optional IntersectionModule::isYieldStuckStatus( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionStopLines & intersection_stoplines, - const TargetObjects & target_objects) const + const intersection::IntersectionStopLines & intersection_stoplines) const { const auto closest_idx = intersection_stoplines.closest_idx; auto fromEgoDist = [&](const size_t index) { return motion_utils::calcSignedArcLength(path.points, closest_idx, index); }; - const auto & intersection_lanelets = intersection_lanelets_.value(); // this is OK - const auto default_stopline_idx = intersection_stoplines.default_stopline.value(); // this is OK - const auto first_attention_stopline_idx = - intersection_stoplines.first_attention_stopline.value(); // this is OK + const auto & intersection_lanelets = intersection_lanelets_.value(); + const auto default_stopline_idx = intersection_stoplines.default_stopline.value(); + const auto first_attention_stopline_idx = intersection_stoplines.first_attention_stopline.value(); const auto stuck_stopline_idx_opt = intersection_stoplines.stuck_stopline; const bool yield_stuck_detected = checkYieldStuckVehicleInIntersection( - target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding()); + interpolated_path_info, intersection_lanelets.attention_non_preceding()); if (yield_stuck_detected) { std::optional stopline_idx = std::nullopt; const bool is_before_default_stopline = fromEgoDist(default_stopline_idx) >= 0.0; @@ -297,14 +295,13 @@ std::optional IntersectionModule::isYieldStuckStat } } if (stopline_idx) { - return intersection::YieldStuckStop{closest_idx, stopline_idx.value()}; + return intersection::YieldStuckStop{closest_idx, stopline_idx.value(), std::string("")}; } } return std::nullopt; } bool IntersectionModule::checkYieldStuckVehicleInIntersection( - const TargetObjects & target_objects, const intersection::InterpolatedPathInfo & interpolated_path_info, const lanelet::ConstLanelets & attention_lanelets) const { @@ -358,19 +355,20 @@ bool IntersectionModule::checkYieldStuckVehicleInIntersection( ::createLaneletFromArcLength(attention_lanelet, yield_stuck_start, yield_stuck_end)); } debug_data_.yield_stuck_detect_area = util::getPolygon3dFromLanelets(yield_stuck_detect_lanelets); - for (const auto & object : target_objects.all_attention_objects) { + for (const auto & object_info : object_info_manager_.attentionObjects()) { + const auto & object = object_info->predicted_object(); const auto obj_v_norm = std::hypot( - object.object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.object.kinematics.initial_twist_with_covariance.twist.linear.y); + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); if (obj_v_norm > stuck_vehicle_vel_thr) { continue; } for (const auto & yield_stuck_detect_lanelet : yield_stuck_detect_lanelets) { const bool is_in_lanelet = lanelet::utils::isInLanelet( - object.object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); + object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); if (is_in_lanelet) { - debug_data_.yield_stuck_targets.objects.push_back(object.object); + debug_data_.yield_stuck_targets.objects.push_back(object); return true; } } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index f103191b2dc6f..e37bb3ee88cc1 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -34,7 +34,6 @@ namespace behavior_velocity_planner::util { /** - * @fn * @brief insert a new pose to the path and return its index * @return if insertion was successful return the inserted point index */ @@ -44,7 +43,6 @@ std::optional insertPointIndex( const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold); /** - * @fn * @brief check if a PathPointWithLaneId contains any of the given lane ids */ bool hasLaneIds( @@ -52,7 +50,6 @@ bool hasLaneIds( const std::set & ids); /** - * @fn * @brief find the first contiguous interval of the path points that contains the specified lane ids * @return if no interval is found, return null. if the interval [start, end] (inclusive range) is * found, returns the pair (start-1, end) @@ -61,7 +58,6 @@ std::optional> findLaneIdsInterval( const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); /** - * @fn * @brief return the index of the first point which is inside the given polygon * @param[in] lane_interval the interval of the path points on the intersection * @param[in] search_forward flag for search direction @@ -72,7 +68,6 @@ std::optional getFirstPointInsidePolygon( const bool search_forward = true); /** - * @fn * @brief check if ego is over the target_idx. If the index is same, compare the exact pose * @param[in] path path * @param[in] closest_idx ego's closest index on the path @@ -84,7 +79,6 @@ bool isOverTargetIndex( const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); /** - * @fn * @brief check if ego is before the target_idx. If the index is same, compare the exact pose * @param[in] path path * @param[in] closest_idx ego's closest index on the path @@ -99,13 +93,11 @@ std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); /** - * @fn * @brief check if the given lane has related traffic light */ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); /** - * @fn * @brief interpolate PathWithLaneId */ std::optional generateInterpolatedPath( @@ -117,7 +109,6 @@ geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state); /** - * @fn * @brief this function sorts the set of lanelets topologically using topological sort and merges * the lanelets from each root to each end. each branch is merged and returned with the original * lanelets @@ -131,7 +122,6 @@ mergeLaneletsByTopologicalSort( const lanelet::routing::RoutingGraphPtr routing_graph_ptr); /** - * @fn * @brief find the index of the first point where vehicle footprint intersects with the given * polygon */ @@ -140,6 +130,10 @@ std::optional getFirstPointInsidePolygonByFootprint( const intersection::InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); +/** + * @brief find the index of the first point where vehicle footprint intersects with the given + * polygons + */ std::optional> getFirstPointInsidePolygonsByFootprint( diff --git a/planning/motion_velocity_smoother/package.xml b/planning/motion_velocity_smoother/package.xml index 4bb0d8a2883dd..9792aa2bdd60b 100644 --- a/planning/motion_velocity_smoother/package.xml +++ b/planning/motion_velocity_smoother/package.xml @@ -8,6 +8,7 @@ Fumiya Watanabe Takamasa Horibe Makoto Kurihara + Satoshi Ota Apache License 2.0 Takamasa Horibe diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 050c4af55c9bf..3bdfd67a7bec4 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -1548,6 +1548,8 @@ void ObstacleStopPlannerNode::filterObstacles( const PredictedObjects & input_objects, const Pose & ego_pose, const TrajectoryPoints & traj, const double dist_threshold, PredictedObjects & filtered_objects) { + if (traj.size() < 2) return; + filtered_objects.header = input_objects.header; for (auto & object : input_objects.objects) { diff --git a/planning/rtc_interface/package.xml b/planning/rtc_interface/package.xml index 78e0f39292075..753e4df13908e 100644 --- a/planning/rtc_interface/package.xml +++ b/planning/rtc_interface/package.xml @@ -7,6 +7,7 @@ Fumiya Watanabe Taiki Tanaka Kyoichi Sugahara + Satoshi Ota Apache License 2.0 diff --git a/sensing/imu_corrector/README.md b/sensing/imu_corrector/README.md index 7af82c1129aff..2df12c94b7d3b 100644 --- a/sensing/imu_corrector/README.md +++ b/sensing/imu_corrector/README.md @@ -70,11 +70,11 @@ In the future, with careful implementation for pose errors, the IMU bias estimat ### Parameters +Note that this node also uses `angular_velocity_offset_x`, `angular_velocity_offset_y`, `angular_velocity_offset_z` parameters from `imu_corrector.param.yaml`. + | Name | Type | Description | | ------------------------------------- | ------ | ------------------------------------------------------------------------------------------- | -| `angular_velocity_offset_x` | double | roll rate offset in imu_link [rad/s] | -| `angular_velocity_offset_y` | double | pitch rate offset imu_link [rad/s] | -| `angular_velocity_offset_z` | double | yaw rate offset imu_link [rad/s] | | `gyro_bias_threshold` | double | threshold of the bias of the gyroscope [rad/s] | | `timer_callback_interval_sec` | double | seconds about the timer callback function [sec] | +| `diagnostics_updater_interval_sec` | double | period of the diagnostics updater [sec] | | `straight_motion_ang_vel_upper_limit` | double | upper limit of yaw angular velocity, beyond which motion is not considered straight [rad/s] | diff --git a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml index e10329c920137..eac423f94fa78 100644 --- a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml +++ b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml @@ -2,4 +2,5 @@ ros__parameters: gyro_bias_threshold: 0.0015 # [rad/s] timer_callback_interval_sec: 0.5 # [sec] + diagnostics_updater_interval_sec: 0.5 # [sec] straight_motion_ang_vel_upper_limit: 0.015 # [rad/s] diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 50e4a702ec949..e99667ed1c4a6 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -30,6 +30,7 @@ GyroBiasEstimator::GyroBiasEstimator() angular_velocity_offset_y_(declare_parameter("angular_velocity_offset_y")), angular_velocity_offset_z_(declare_parameter("angular_velocity_offset_z")), timer_callback_interval_sec_(declare_parameter("timer_callback_interval_sec")), + diagnostics_updater_interval_sec_(declare_parameter("diagnostics_updater_interval_sec")), straight_motion_ang_vel_upper_limit_( declare_parameter("straight_motion_ang_vel_upper_limit")), updater_(this), @@ -37,6 +38,8 @@ GyroBiasEstimator::GyroBiasEstimator() { updater_.setHardwareID(get_name()); updater_.add("gyro_bias_validator", this, &GyroBiasEstimator::update_diagnostics); + // diagnostic_updater is designed to be updated at the same rate as the timer + updater_.setPeriod(diagnostics_updater_interval_sec_); gyro_bias_estimation_module_ = std::make_unique(); @@ -57,6 +60,18 @@ GyroBiasEstimator::GyroBiasEstimator() this->get_node_timers_interface()->add_timer(timer_, nullptr); transform_listener_ = std::make_shared(this); + + // initialize diagnostics_info_ + { + diagnostics_info_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diagnostics_info_.summary_message = "Not initialized"; + diagnostics_info_.gyro_bias_x_for_imu_corrector = std::nan(""); + diagnostics_info_.gyro_bias_y_for_imu_corrector = std::nan(""); + diagnostics_info_.gyro_bias_z_for_imu_corrector = std::nan(""); + diagnostics_info_.estimated_gyro_bias_x = std::nan(""); + diagnostics_info_.estimated_gyro_bias_y = std::nan(""); + diagnostics_info_.estimated_gyro_bias_z = std::nan(""); + } } void GyroBiasEstimator::callback_imu(const Imu::ConstSharedPtr imu_msg_ptr) @@ -99,6 +114,7 @@ void GyroBiasEstimator::callback_odom(const Odometry::ConstSharedPtr odom_msg_pt void GyroBiasEstimator::timer_callback() { if (pose_buf_.empty()) { + diagnostics_info_.summary_message = "Skipped update (pose_buf is empty)"; return; } @@ -112,6 +128,7 @@ void GyroBiasEstimator::timer_callback() const rclcpp::Time t0_rclcpp_time = rclcpp::Time(pose_buf.front().header.stamp); const rclcpp::Time t1_rclcpp_time = rclcpp::Time(pose_buf.back().header.stamp); if (t1_rclcpp_time <= t0_rclcpp_time) { + diagnostics_info_.summary_message = "Skipped update (pose_buf is not in chronological order)"; return; } @@ -127,6 +144,7 @@ void GyroBiasEstimator::timer_callback() // Check gyro data size // Data size must be greater than or equal to 2 since the time difference will be taken later if (gyro_filtered.size() <= 1) { + diagnostics_info_.summary_message = "Skipped update (gyro_filtered size is less than 2)"; return; } @@ -140,6 +158,8 @@ void GyroBiasEstimator::timer_callback() const double yaw_vel = yaw_diff / time_diff; const bool is_straight = (yaw_vel < straight_motion_ang_vel_upper_limit_); if (!is_straight) { + diagnostics_info_.summary_message = + "Skipped update (yaw angular velocity is greater than straight_motion_ang_vel_upper_limit)"; return; } @@ -151,12 +171,45 @@ void GyroBiasEstimator::timer_callback() if (!tf_base2imu_ptr) { RCLCPP_ERROR( this->get_logger(), "Please publish TF %s to %s", imu_frame_.c_str(), output_frame_.c_str()); + + diagnostics_info_.summary_message = "Skipped update (tf between base and imu is not available)"; return; } + gyro_bias_ = transform_vector3(gyro_bias_estimation_module_->get_bias_base_link(), *tf_base2imu_ptr); + validate_gyro_bias(); updater_.force_update(); + updater_.setPeriod(diagnostics_updater_interval_sec_); // to reset timer inside the updater +} + +void GyroBiasEstimator::validate_gyro_bias() +{ + // Calculate diagnostics key-values + diagnostics_info_.gyro_bias_x_for_imu_corrector = gyro_bias_.value().x; + diagnostics_info_.gyro_bias_y_for_imu_corrector = gyro_bias_.value().y; + diagnostics_info_.gyro_bias_z_for_imu_corrector = gyro_bias_.value().z; + diagnostics_info_.estimated_gyro_bias_x = gyro_bias_.value().x - angular_velocity_offset_x_; + diagnostics_info_.estimated_gyro_bias_y = gyro_bias_.value().y - angular_velocity_offset_y_; + diagnostics_info_.estimated_gyro_bias_z = gyro_bias_.value().z - angular_velocity_offset_z_; + + // Validation + const bool is_bias_small_enough = + std::abs(diagnostics_info_.estimated_gyro_bias_x) < gyro_bias_threshold_ && + std::abs(diagnostics_info_.estimated_gyro_bias_y) < gyro_bias_threshold_ && + std::abs(diagnostics_info_.estimated_gyro_bias_z) < gyro_bias_threshold_; + + // Update diagnostics + if (is_bias_small_enough) { + diagnostics_info_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diagnostics_info_.summary_message = "Successfully updated"; + } else { + diagnostics_info_.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diagnostics_info_.summary_message = + "Gyro bias may be incorrect. Please calibrate IMU and reflect the result in imu_corrector. " + "You may also use the output of gyro_bias_estimator."; + } } geometry_msgs::msg::Vector3 GyroBiasEstimator::transform_vector3( @@ -172,36 +225,22 @@ geometry_msgs::msg::Vector3 GyroBiasEstimator::transform_vector3( void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) { - if (gyro_bias_ == std::nullopt) { - stat.add("gyro_bias", "Bias estimation is not yet ready because of insufficient data."); - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Not initialized"); - } else { - stat.add("gyro_bias_x_for_imu_corrector", gyro_bias_.value().x); - stat.add("gyro_bias_y_for_imu_corrector", gyro_bias_.value().y); - stat.add("gyro_bias_z_for_imu_corrector", gyro_bias_.value().z); - - stat.add("estimated_gyro_bias_x", gyro_bias_.value().x - angular_velocity_offset_x_); - stat.add("estimated_gyro_bias_y", gyro_bias_.value().y - angular_velocity_offset_y_); - stat.add("estimated_gyro_bias_z", gyro_bias_.value().z - angular_velocity_offset_z_); - - // Validation - const bool is_bias_small_enough = - std::abs(gyro_bias_.value().x - angular_velocity_offset_x_) < gyro_bias_threshold_ && - std::abs(gyro_bias_.value().y - angular_velocity_offset_y_) < gyro_bias_threshold_ && - std::abs(gyro_bias_.value().z - angular_velocity_offset_z_) < gyro_bias_threshold_; - - // Update diagnostics - if (is_bias_small_enough) { - stat.add("gyro_bias", "OK"); - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "OK"); - } else { - stat.add( - "gyro_bias", - "Gyro bias may be incorrect. Please calibrate IMU and reflect the result in " - "imu_corrector. You may also use the output of gyro_bias_estimator."); - stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "WARN"); - } - } + auto f = [](const double & value) { + std::stringstream ss; + ss << std::fixed << std::setprecision(8) << value; + return ss.str(); + }; + + stat.summary(diagnostics_info_.level, diagnostics_info_.summary_message); + stat.add("gyro_bias_x_for_imu_corrector", f(diagnostics_info_.gyro_bias_x_for_imu_corrector)); + stat.add("gyro_bias_y_for_imu_corrector", f(diagnostics_info_.gyro_bias_y_for_imu_corrector)); + stat.add("gyro_bias_z_for_imu_corrector", f(diagnostics_info_.gyro_bias_z_for_imu_corrector)); + + stat.add("estimated_gyro_bias_x", f(diagnostics_info_.estimated_gyro_bias_x)); + stat.add("estimated_gyro_bias_y", f(diagnostics_info_.estimated_gyro_bias_y)); + stat.add("estimated_gyro_bias_z", f(diagnostics_info_.estimated_gyro_bias_z)); + + stat.add("gyro_bias_threshold", f(gyro_bias_threshold_)); } } // namespace imu_corrector diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp index 821cba0b213ff..3592ff1f7d3b4 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.hpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.hpp @@ -49,6 +49,7 @@ class GyroBiasEstimator : public rclcpp::Node void callback_imu(const Imu::ConstSharedPtr imu_msg_ptr); void callback_odom(const Odometry::ConstSharedPtr odom_msg_ptr); void timer_callback(); + void validate_gyro_bias(); static geometry_msgs::msg::Vector3 transform_vector3( const geometry_msgs::msg::Vector3 & vec, @@ -68,6 +69,7 @@ class GyroBiasEstimator : public rclcpp::Node const double angular_velocity_offset_y_; const double angular_velocity_offset_z_; const double timer_callback_interval_sec_; + const double diagnostics_updater_interval_sec_; const double straight_motion_ang_vel_upper_limit_; diagnostic_updater::Updater updater_; @@ -80,6 +82,20 @@ class GyroBiasEstimator : public rclcpp::Node std::vector gyro_all_; std::vector pose_buf_; + + struct DiagnosticsInfo + { + unsigned char level; + std::string summary_message; + double gyro_bias_x_for_imu_corrector; + double gyro_bias_y_for_imu_corrector; + double gyro_bias_z_for_imu_corrector; + double estimated_gyro_bias_x; + double estimated_gyro_bias_y; + double estimated_gyro_bias_z; + }; + + DiagnosticsInfo diagnostics_info_; }; } // namespace imu_corrector diff --git a/sensing/radar_scan_to_pointcloud2/package.xml b/sensing/radar_scan_to_pointcloud2/package.xml index 2360e8b33901d..a9d4fe7ebda8f 100644 --- a/sensing/radar_scan_to_pointcloud2/package.xml +++ b/sensing/radar_scan_to_pointcloud2/package.xml @@ -5,6 +5,9 @@ radar_scan_to_pointcloud2 Satoshi Tanaka Shunsuke Miura + Yoshi Ri + Taekjin Lee + Satoshi Tanaka Apache License 2.0 diff --git a/sensing/radar_static_pointcloud_filter/package.xml b/sensing/radar_static_pointcloud_filter/package.xml index a15aa43d71cf6..05453ee9cb9e0 100644 --- a/sensing/radar_static_pointcloud_filter/package.xml +++ b/sensing/radar_static_pointcloud_filter/package.xml @@ -5,6 +5,9 @@ radar_static_pointcloud_filter Satoshi Tanaka Shunsuke Miura + Yoshi Ri + Taekjin Lee + Satoshi Tanaka Apache License 2.0 diff --git a/sensing/radar_threshold_filter/package.xml b/sensing/radar_threshold_filter/package.xml index 00bb530567dc4..6b81f225db971 100644 --- a/sensing/radar_threshold_filter/package.xml +++ b/sensing/radar_threshold_filter/package.xml @@ -5,6 +5,9 @@ radar_threshold_filter Satoshi Tanaka Shunsuke Miura + Yoshi Ri + Taekjin Lee + Satoshi Tanaka Apache License 2.0 diff --git a/sensing/radar_tracks_noise_filter/CMakeLists.txt b/sensing/radar_tracks_noise_filter/CMakeLists.txt index 517a3f1df72f1..56cbdb98fdbc8 100644 --- a/sensing/radar_tracks_noise_filter/CMakeLists.txt +++ b/sensing/radar_tracks_noise_filter/CMakeLists.txt @@ -28,13 +28,21 @@ rclcpp_components_register_node(radar_tracks_noise_filter_node_component # Tests if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + file(GLOB_RECURSE test_files test/**/*.cpp) + ament_add_ros_isolated_gtest(radar_tracks_noise_filter ${test_files}) + + target_link_libraries(radar_tracks_noise_filter + radar_tracks_noise_filter_node_component + ) endif() # Package ament_auto_package( INSTALL_TO_SHARE + config launch ) diff --git a/sensing/radar_tracks_noise_filter/config/radar_tracks_noise_filter.param.yaml b/sensing/radar_tracks_noise_filter/config/radar_tracks_noise_filter.param.yaml new file mode 100644 index 0000000000000..0d7ddd345b03d --- /dev/null +++ b/sensing/radar_tracks_noise_filter/config/radar_tracks_noise_filter.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + velocity_y_threshold: 7.0 diff --git a/sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp b/sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp index 9b329da3e5579..2ff4025cc64bc 100644 --- a/sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp +++ b/sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp @@ -57,6 +57,7 @@ class RadarTrackCrossingNoiseFilterNode : public rclcpp::Node // Parameter NodeParam node_param_{}; +public: // Core bool isNoise(const RadarTrack & radar_track); }; diff --git a/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml b/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml index f38fe36b50af4..028ca5de6a220 100644 --- a/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml +++ b/sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml @@ -1,17 +1,13 @@ - - - - + - - + diff --git a/sensing/radar_tracks_noise_filter/package.xml b/sensing/radar_tracks_noise_filter/package.xml index 3af9658535687..8420b7a174edf 100644 --- a/sensing/radar_tracks_noise_filter/package.xml +++ b/sensing/radar_tracks_noise_filter/package.xml @@ -6,6 +6,9 @@ radar_tracks_noise_filter Sathshi Tanaka Shunsuke Miura + Yoshi Ri + Taekjin Lee + Apache License 2.0 Sathshi Tanaka diff --git a/sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp b/sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp new file mode 100644 index 0000000000000..aa08260dee2e7 --- /dev/null +++ b/sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp @@ -0,0 +1,95 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp" + +#include + +#include + +std::shared_ptr get_node( + float velocity_y_threshold) +{ + rclcpp::NodeOptions node_options; + node_options.parameter_overrides({ + {"node_params.is_amplitude_filter", true}, + {"velocity_y_threshold", velocity_y_threshold}, + }); + + auto node = + std::make_shared(node_options); + return node; +} + +radar_msgs::msg::RadarTrack getRadarTrack(float velocity_x, float velocity_y) +{ + radar_msgs::msg::RadarTrack radar_track; + geometry_msgs::msg::Vector3 vector_msg; + vector_msg.x = velocity_x; + vector_msg.y = velocity_y; + vector_msg.z = 0.0; + radar_track.velocity = vector_msg; + return radar_track; +} + +TEST(RadarTracksNoiseFilter, isNoise) +{ + using radar_msgs::msg::RadarTrack; + using radar_tracks_noise_filter::RadarTrackCrossingNoiseFilterNode; + rclcpp::init(0, nullptr); + { + float velocity_node_threshold = 0.0; + float y_velocity = 0.0; + float x_velocity = 10.0; + std::shared_ptr node = get_node(velocity_node_threshold); + RadarTrack radar_track = getRadarTrack(x_velocity, y_velocity); + EXPECT_TRUE(node->isNoise(radar_track)); + } + + { + float velocity_node_threshold = -1.0; + float y_velocity = 3.0; + float x_velocity = 10.0; + std::shared_ptr node = get_node(velocity_node_threshold); + RadarTrack radar_track = getRadarTrack(x_velocity, y_velocity); + EXPECT_TRUE(node->isNoise(radar_track)); + } + + { + float velocity_node_threshold = -1.0; + float y_velocity = 3.0; + float x_velocity = 100.0; + std::shared_ptr node = get_node(velocity_node_threshold); + RadarTrack radar_track = getRadarTrack(x_velocity, y_velocity); + EXPECT_TRUE(node->isNoise(radar_track)); + } + + { + float velocity_node_threshold = 3.0; + float y_velocity = 1.0; + float x_velocity = 10.0; + std::shared_ptr node = get_node(velocity_node_threshold); + RadarTrack radar_track = getRadarTrack(x_velocity, y_velocity); + EXPECT_FALSE(node->isNoise(radar_track)); + } + + { + float velocity_node_threshold = 3.0; + float y_velocity = 1.0; + float x_velocity = -10.0; + std::shared_ptr node = get_node(velocity_node_threshold); + RadarTrack radar_track = getRadarTrack(x_velocity, y_velocity); + EXPECT_FALSE(node->isNoise(radar_track)); + } +} diff --git a/sensing/radar_tracks_noise_filter/test/test_radar_tracks_noise_filter.cpp b/sensing/radar_tracks_noise_filter/test/test_radar_tracks_noise_filter.cpp new file mode 100644 index 0000000000000..55032a7b62e79 --- /dev/null +++ b/sensing/radar_tracks_noise_filter/test/test_radar_tracks_noise_filter.cpp @@ -0,0 +1,26 @@ +// Copyright 2023 TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/simulator/fault_injection/config/fault_injection.param.yaml b/simulator/fault_injection/config/fault_injection.param.yaml index 1a57b852f7361..ac02442d70b4d 100644 --- a/simulator/fault_injection/config/fault_injection.param.yaml +++ b/simulator/fault_injection/config/fault_injection.param.yaml @@ -4,7 +4,7 @@ vehicle_is_out_of_lane: "lane_departure" trajectory_deviation_is_high: "trajectory_deviation" localization_matching_score_is_low: "ndt_scan_matcher" - localization_accuracy_is_low: "localization_accuracy" + localization_accuracy_is_low: "localization_error_ellipse" map_version_is_different: "map_version" trajectory_is_invalid: "trajectory_point_validation" cpu_temperature_is_high: "CPU Temperature" diff --git a/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml index 55af6ab2d2c55..9ed82304036cc 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml @@ -24,9 +24,9 @@ contains: ["ndt_scan_matcher"] timeout: 1.0 - localization_accuracy: + localization_error_ellipse: type: diagnostic_aggregator/GenericAnalyzer - path: localization_accuracy + path: localization_error_ellipse contains: ["localization: localization_error_monitor"] timeout: 1.0 diff --git a/system/system_error_monitor/config/system_error_monitor.param.yaml b/system/system_error_monitor/config/system_error_monitor.param.yaml index 1778f6594f0c3..2b58f5b42c0be 100644 --- a/system/system_error_monitor/config/system_error_monitor.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.param.yaml @@ -24,7 +24,7 @@ /autoware/localization/node_alive_monitoring: default /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } - /autoware/localization/performance_monitoring/localization_accuracy: default + /autoware/localization/performance_monitoring/localization_error_ellipse: default /autoware/map/node_alive_monitoring: default diff --git a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml index 9784259490ec2..f1031444394d0 100644 --- a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml @@ -24,7 +24,7 @@ /autoware/localization/node_alive_monitoring: default # /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } - # /autoware/localization/performance_monitoring/localization_accuracy: default + # /autoware/localization/performance_monitoring/localization_error_ellipse: default /autoware/map/node_alive_monitoring: default