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/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/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 3bd3aff10a4e4..e8c19382864ba 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -16,8 +16,8 @@ - - + + 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_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/perception/simple_object_merger/README.md b/perception/simple_object_merger/README.md index 9bcd03e8abfa1..4c037963361a1 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 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..ccd038c23b257 100644 --- a/perception/simple_object_merger/launch/simple_object_merger.launch.xml +++ b/perception/simple_object_merger/launch/simple_object_merger.launch.xml @@ -4,7 +4,7 @@ - + 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/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_fine_detector/launch/traffic_light_fine_detector.launch.xml b/perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml index e7a68ea9b9e94..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 @@ - + 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/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 5fe47715bff08..b5d351cbc48a8 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; @@ -723,6 +725,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 +1637,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; 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 8d2e3b451fe98..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,6 +53,8 @@ class NormalLaneChange : public LaneChangeBase LaneChangePath getLaneChangePath() const override; + BehaviorModuleOutput getTerminalLaneChangePath() const override; + BehaviorModuleOutput generateOutput() override; void extendOutputDrivableArea(BehaviorModuleOutput & output) const override; @@ -141,6 +143,10 @@ class NormalLaneChange : public LaneChangeBase const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety = true) const 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 e78d706334bae..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; @@ -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; } @@ -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..9c04ff4c67c58 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -83,7 +83,7 @@ void LaneChangeInterface::updateData() if (isWaitingApproval()) { module_type_->updateLaneChangeStatus(); } - setObjectDebugVisualization(); + updateDebugMarker(); module_type_->updateSpecialData(); module_type_->resetStopPose(); @@ -109,7 +109,8 @@ 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); } @@ -123,17 +124,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 +290,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 +311,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 +435,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 b115ab7c163c0..c5a0f21d0f909 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,46 @@ LaneChangePath NormalLaneChange::getLaneChangePath() const return status_.lane_change_path; } +BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const +{ + BehaviorModuleOutput 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_WARN(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; @@ -421,13 +461,8 @@ 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."); } @@ -840,6 +875,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 +948,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 +1169,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 +1215,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 +1442,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 +1458,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 +1475,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_WARN(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_WARN(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_WARN(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_WARN( + 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_WARN(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 +1627,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,16 +1635,17 @@ 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; } } @@ -1467,6 +1672,10 @@ TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() const 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; @@ -1802,7 +2011,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_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/src/decision_result.cpp b/planning/behavior_velocity_intersection_module/src/decision_result.cpp index 93a7c2a29d2f7..aefd59a72f9b4 100644 --- a/planning/behavior_velocity_intersection_module/src/decision_result.cpp +++ b/planning/behavior_velocity_intersection_module/src/decision_result.cpp @@ -19,11 +19,11 @@ 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); + 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); + const auto & state = std::get(decision_result); return "OverPassJudge:\nsafety_report:" + state.safety_report + "\nevasive_report:\n" + state.evasive_report; } @@ -31,11 +31,11 @@ std::string formatDecisionResult(const DecisionResult & decision_result) return "StuckStop"; } if (std::holds_alternative(decision_result)) { - const auto state = std::get(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); + const auto & state = std::get(decision_result); return "NonOccludedCollisionStop\nsafety_report:" + state.safety_report; } if (std::holds_alternative(decision_result)) { @@ -45,18 +45,18 @@ std::string formatDecisionResult(const DecisionResult & decision_result) return "PeekingTowardOcclusion"; } if (std::holds_alternative(decision_result)) { - const auto state = std::get(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); + 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); + const auto & state = std::get(decision_result); return "FullyPrioritized\nsafety_report:" + state.safety_report; } return ""; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 75e2da034780a..3f6136673a44a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -354,8 +354,18 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( : false; if (!has_traffic_light_) { if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) { - return intersection::InternalError{ - "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, @@ -364,7 +374,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( closest_idx, first_attention_stopline_idx, occlusion_wo_tl_pass_judge_line_idx, - safety_report}; + safety_diag}; } // ========================================================================================== @@ -1251,7 +1261,22 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat 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); 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 4f7e8b45d107f..4496df77134e2 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -621,6 +621,9 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( 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" @@ -910,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(), 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_tracks_noise_filter/CMakeLists.txt b/sensing/radar_tracks_noise_filter/CMakeLists.txt index 517a3f1df72f1..133a057cf1a12 100644 --- a/sensing/radar_tracks_noise_filter/CMakeLists.txt +++ b/sensing/radar_tracks_noise_filter/CMakeLists.txt @@ -28,9 +28,16 @@ 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 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/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; +}