diff --git a/README.md b/README.md index 031ac62..af1b509 100644 --- a/README.md +++ b/README.md @@ -1 +1 @@ -# autoware_rviz_plugins \ No newline at end of file +# autoware_rviz_plugins diff --git a/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/CMakeLists.txt b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000..8692ea1 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/CMakeLists.txt @@ -0,0 +1,108 @@ +cmake_minimum_required(VERSION 3.8) +project(autoware_mission_details_overlay_rviz_plugin) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +set( + headers_to_moc + include/mission_details_display.hpp +) + +set( + headers_to_not_moc + include/remaining_distance_time_display.hpp +) + +foreach(header "${headers_to_moc}") + qt5_wrap_cpp(display_moc_files "${header}") +endforeach() + +set( + display_source_files + src/overlay_utils.cpp + src/mission_details_display.cpp + src/remaining_distance_time_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 + autoware_internal_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 "autoware_mission_details_overlay_rviz_plugin-extras.cmake" +) diff --git a/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/LICENSE b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/LICENSE new file mode 100644 index 0000000..a2fe2d3 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/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/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/README.md b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/README.md new file mode 100644 index 0000000..3a4040a --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/README.md @@ -0,0 +1,35 @@ +# autoware_mission_details_overlay_rviz_plugin + +This RViz plugin displays the remaining distance and time for the current mission. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------------------------- | ----------------------------------------------------------- | ---------------------------------------------------- | +| `/planning/mission_remaining_distance_time` | `autoware_planning_msgs::msg::MissionRemainingDistanceTime` | The topic is for mission remaining distance and time | + +## Overlay Parameters + +| Name | Type | Default Value | Description | +| -------- | ---- | ------------- | --------------------------------- | +| `Width` | int | 170 | Width of the overlay [px] | +| `Height` | int | 100 | Height of the overlay [px] | +| `Right` | int | 10 | Margin from the right border [px] | +| `Top` | int | 10 | Margin from the top border [px] | + +The mission details display is aligned with top right corner of the screen. + +## Usage + +Similar to [autoware_overlay_rviz_plugin](../autoware_overlay_rviz_plugin/README.md) + +## Credits + +Based on the [jsk_visualization](https://github.com/jsk-ros-pkg/jsk_visualization) package. + +### Icons + +- +- diff --git a/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/av_timer.png b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/av_timer.png new file mode 100644 index 0000000..f7f5db1 Binary files /dev/null and b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/av_timer.png differ diff --git a/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/font/Quicksand/LICENSE b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/font/Quicksand/LICENSE new file mode 100644 index 0000000..cc04d39 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/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/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Bold.ttf b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Bold.ttf new file mode 100644 index 0000000..07d5127 Binary files /dev/null and b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Bold.ttf differ diff --git a/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Regular.ttf b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Regular.ttf new file mode 100644 index 0000000..60323ed Binary files /dev/null and b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Regular.ttf differ diff --git a/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/path.png b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/path.png new file mode 100644 index 0000000..62af338 Binary files /dev/null and b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/path.png differ diff --git a/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin-extras.cmake b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin-extras.cmake new file mode 100644 index 0000000..9d6f156 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin-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/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/mission_details_display.hpp b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/mission_details_display.hpp new file mode 100644 index 0000000..c3db6a2 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/mission_details_display.hpp @@ -0,0 +1,87 @@ +// 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 MISSION_DETAILS_DISPLAY_HPP_ +#define MISSION_DETAILS_DISPLAY_HPP_ +#ifndef Q_MOC_RUN +#include "overlay_utils.hpp" +#include "remaining_distance_time_display.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#endif + +namespace autoware::mission_details_overlay_rviz_plugin +{ +class MissionDetailsDisplay : public rviz_common::Display +{ + Q_OBJECT +public: + MissionDetailsDisplay(); + ~MissionDetailsDisplay() 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 update_size(); + void topic_updated_remaining_distance_time(); + +private: + std::mutex mutex_; + autoware::mission_details_overlay_rviz_plugin::OverlayObject::SharedPtr overlay_; + rviz_common::properties::IntProperty * property_width_; + rviz_common::properties::IntProperty * property_height_; + rviz_common::properties::IntProperty * property_right_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::ColorProperty * property_bg_color_; + rviz_common::properties::FloatProperty * property_bg_alpha_; + rviz_common::properties::ColorProperty * property_text_color_; + + std::unique_ptr + remaining_distance_time_topic_property_; + + void draw_rounded_rect(QPainter & painter, const QRectF & backgroundRect); + void setupRosSubscriptions(); + + std::unique_ptr remaining_distance_time_display_; + + rclcpp::Subscription::SharedPtr + remaining_distance_time_sub_; + + std::mutex property_mutex_; + + void cb_remaining_distance_time( + const autoware_internal_msgs::msg::MissionRemainingDistanceTime::ConstSharedPtr & msg); + void draw_widget(QImage & hud); +}; +} // namespace autoware::mission_details_overlay_rviz_plugin + +#endif // MISSION_DETAILS_DISPLAY_HPP_ diff --git a/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/overlay_utils.hpp b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/overlay_utils.hpp new file mode 100644 index 0000000..a0344f8 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/overlay_utils.hpp @@ -0,0 +1,131 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace autoware::mission_details_overlay_rviz_plugin +{ +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, TOP, BOTTOM }; + +enum class HorizontalAlignment : uint8_t { LEFT, RIGHT, 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 autoware::mission_details_overlay_rviz_plugin + +#endif // OVERLAY_UTILS_HPP_ diff --git a/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/remaining_distance_time_display.hpp b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/remaining_distance_time_display.hpp new file mode 100644 index 0000000..d3f246f --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/remaining_distance_time_display.hpp @@ -0,0 +1,56 @@ +// 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 REMAINING_DISTANCE_TIME_DISPLAY_HPP_ +#define REMAINING_DISTANCE_TIME_DISPLAY_HPP_ +#include "overlay_utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace autoware::mission_details_overlay_rviz_plugin +{ + +class RemainingDistanceTimeDisplay +{ +public: + RemainingDistanceTimeDisplay(); + void drawRemainingDistanceTimeDisplay( + QPainter & painter, const QRectF & backgroundRect, const QColor & text_color); + void updateRemainingDistanceTimeData( + const autoware_internal_msgs::msg::MissionRemainingDistanceTime::ConstSharedPtr & msg); + +private: + double remaining_distance_; // Internal variable to store remaining distance + double remaining_time_; // Internal variable to store remaining time + + QImage icon_dist_; + QImage icon_dist_scaled_; + QImage icon_time_; + QImage icon_time_scaled_; +}; + +} // namespace autoware::mission_details_overlay_rviz_plugin + +#endif // REMAINING_DISTANCE_TIME_DISPLAY_HPP_ diff --git a/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/package.xml b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/package.xml new file mode 100644 index 0000000..87c42d0 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/package.xml @@ -0,0 +1,28 @@ + + + + autoware_mission_details_overlay_rviz_plugin + 0.0.0 + + RViz2 plugin for 2D overlays for mission details in the 3D view. Mainly a port of the JSK overlay plugin + (https://github.com/jsk-ros-pkg/jsk_visualization). + + Ahmed Ebrahim + + BSD-3-Clause + + autoware_internal_msgs + boost + rviz_common + rviz_ogre_vendor + rviz_rendering + + ament_lint_auto + autoware_lint_common + + ament_cmake_auto + + + ament_cmake + + diff --git a/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/plugins_description.xml b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/plugins_description.xml new file mode 100644 index 0000000..5177b10 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/plugins_description.xml @@ -0,0 +1,5 @@ + + + Mission Details overlay plugin for the 3D view. + + diff --git a/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/mission_details_display.cpp b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/mission_details_display.cpp new file mode 100644 index 0000000..2e76a2a --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/mission_details_display.cpp @@ -0,0 +1,221 @@ +// 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 "mission_details_display.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::mission_details_overlay_rviz_plugin +{ + +MissionDetailsDisplay::MissionDetailsDisplay() +{ + property_width_ = new rviz_common::properties::IntProperty( + "Width", 170, "Width of the overlay", this, SLOT(update_size())); + property_height_ = new rviz_common::properties::IntProperty( + "Height", 100, "Height of the overlay", this, SLOT(update_size())); + property_right_ = new rviz_common::properties::IntProperty( + "Right", 10, "Margin from the right border", this, SLOT(update_size())); + property_top_ = new rviz_common::properties::IntProperty( + "Top", 10, "Margin from the top border", this, SLOT(update_size())); + property_bg_alpha_ = new rviz_common::properties::FloatProperty( + "Background Alpha", 0.3, "Background Alpha", this, SLOT(update_size())); + property_bg_color_ = new rviz_common::properties::ColorProperty( + "Background Color", QColor(0, 0, 0), "Background Color", this, SLOT(update_size())); + property_text_color_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(194, 194, 194), "Text Color", this, SLOT(update_size())); + + // Initialize the component displays + remaining_distance_time_display_ = std::make_unique(); +} + +void MissionDetailsDisplay::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 << "MissionDetailsDisplay" << count++; + overlay_ = + std::make_shared(ss.str()); + overlay_->show(); + update_size(); + + auto rviz_ros_node = context_->getRosNodeAbstraction(); + + remaining_distance_time_topic_property_ = + std::make_unique( + "Remaining Distance and Time Topic", "/planning/mission_remaining_distance_time", + "autoware_internal_msgs/msg/MissionRemainingDistanceTime", + "Topic for Mission Remaining Distance and Time Data", this, + SLOT(topic_updated_remaining_distance_time())); + remaining_distance_time_topic_property_->initialize(rviz_ros_node); +} + +void MissionDetailsDisplay::setupRosSubscriptions() +{ + // Don't create a node, just use the one from the parent class + auto rviz_node_ = context_->getRosNodeAbstraction().lock()->get_raw_node(); + + remaining_distance_time_sub_ = + rviz_node_->create_subscription( + remaining_distance_time_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_internal_msgs::msg::MissionRemainingDistanceTime::SharedPtr msg) { + cb_remaining_distance_time(msg); + }); +} + +MissionDetailsDisplay::~MissionDetailsDisplay() +{ + std::lock_guard lock(property_mutex_); + overlay_.reset(); + + remaining_distance_time_sub_.reset(); + remaining_distance_time_display_.reset(); + remaining_distance_time_topic_property_.reset(); +} + +// mark maybe unused +void MissionDetailsDisplay::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; // Mark as unused + (void)ros_dt; // Mark as unused + + if (!overlay_) { + return; + } + autoware::mission_details_overlay_rviz_plugin::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(Qt::transparent); + draw_widget(hud); +} + +void MissionDetailsDisplay::onEnable() +{ + std::lock_guard lock(property_mutex_); + if (overlay_) { + overlay_->show(); + } + setupRosSubscriptions(); +} + +void MissionDetailsDisplay::onDisable() +{ + std::lock_guard lock(property_mutex_); + + remaining_distance_time_sub_.reset(); + + if (overlay_) { + overlay_->hide(); + } +} + +void MissionDetailsDisplay::cb_remaining_distance_time( + const autoware_internal_msgs::msg::MissionRemainingDistanceTime::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (remaining_distance_time_display_) { + remaining_distance_time_display_->updateRemainingDistanceTimeData(msg); + queueRender(); + } +} + +void MissionDetailsDisplay::draw_widget(QImage & hud) +{ + std::lock_guard lock(property_mutex_); + + if (!overlay_->isVisible()) { + return; + } + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + QRectF backgroundRect(0, 0, qreal(property_width_->getInt()), qreal(property_height_->getInt())); + draw_rounded_rect(painter, backgroundRect); + + if (remaining_distance_time_display_) { + remaining_distance_time_display_->drawRemainingDistanceTimeDisplay( + painter, backgroundRect, property_text_color_->getColor()); + } + + painter.end(); +} + +void MissionDetailsDisplay::draw_rounded_rect(QPainter & painter, const QRectF & backgroundRect) +{ + painter.setRenderHint(QPainter::Antialiasing, true); + QColor colorFromHSV; + colorFromHSV.setHsv( + property_bg_color_->getColor().hue(), property_bg_color_->getColor().saturation(), + property_bg_color_->getColor().value()); + colorFromHSV.setAlphaF(property_bg_alpha_->getFloat()); + + painter.setBrush(colorFromHSV); + + painter.setPen(Qt::NoPen); + painter.drawRoundedRect(backgroundRect, backgroundRect.height() / 2, backgroundRect.height() / 2); +} + +void MissionDetailsDisplay::reset() +{ + rviz_common::Display::reset(); + overlay_->hide(); +} + +void MissionDetailsDisplay::update_size() +{ + std::lock_guard lock(mutex_); + overlay_->updateTextureSize(property_width_->getInt(), property_height_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); + overlay_->setPosition( + property_right_->getInt(), property_top_->getInt(), HorizontalAlignment::RIGHT, + VerticalAlignment::TOP); + queueRender(); +} + +void MissionDetailsDisplay::topic_updated_remaining_distance_time() +{ + // resubscribe to the topic + remaining_distance_time_sub_.reset(); + auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); + remaining_distance_time_sub_ = + rviz_ros_node->get_raw_node() + ->create_subscription( + remaining_distance_time_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_internal_msgs::msg::MissionRemainingDistanceTime::SharedPtr msg) { + cb_remaining_distance_time(msg); + }); +} + +} // namespace autoware::mission_details_overlay_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::mission_details_overlay_rviz_plugin::MissionDetailsDisplay, rviz_common::Display) diff --git a/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/overlay_utils.cpp b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/overlay_utils.cpp new file mode 100644 index 0000000..7989dd2 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/overlay_utils.cpp @@ -0,0 +1,269 @@ +// 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 + +#include + +namespace autoware::mission_details_overlay_rviz_plugin +{ +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 autoware::mission_details_overlay_rviz_plugin diff --git a/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/remaining_distance_time_display.cpp b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/remaining_distance_time_display.cpp new file mode 100644 index 0000000..41ee64f --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/remaining_distance_time_display.cpp @@ -0,0 +1,158 @@ +// 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 "remaining_distance_time_display.hpp" + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace autoware::mission_details_overlay_rviz_plugin +{ + +RemainingDistanceTimeDisplay::RemainingDistanceTimeDisplay() +: remaining_distance_(0.0), remaining_time_(0.0) +{ + std::string package_path = + ament_index_cpp::get_package_share_directory("autoware_mission_details_overlay_rviz_plugin"); + 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."; + } + + std::string dist_image = package_path + "/assets/path.png"; + std::string time_image = package_path + "/assets/av_timer.png"; + icon_dist_.load(dist_image.c_str()); + icon_time_.load(time_image.c_str()); + icon_dist_scaled_ = icon_dist_.scaled(28, 28, Qt::KeepAspectRatio, Qt::SmoothTransformation); + icon_time_scaled_ = icon_time_.scaled(28, 28, Qt::KeepAspectRatio, Qt::SmoothTransformation); +} + +void RemainingDistanceTimeDisplay::updateRemainingDistanceTimeData( + const autoware_internal_msgs::msg::MissionRemainingDistanceTime::ConstSharedPtr & msg) +{ + remaining_distance_ = msg->remaining_distance; + remaining_time_ = msg->remaining_time; +} + +void RemainingDistanceTimeDisplay::drawRemainingDistanceTimeDisplay( + QPainter & painter, const QRectF & backgroundRect, const QColor & text_color) +{ + const QFont font("Quicksand", 15, QFont::Bold); + painter.setFont(font); + + // We'll display the distance and time in two rows + + auto calculate_inner_rect = [](const QRectF & outer_rect, double ratio_x, double ratio_y) { + QSizeF size_inner(outer_rect.width() * ratio_x, outer_rect.height() * ratio_y); + QPointF top_left_inner = QPointF( + outer_rect.center().x() - size_inner.width() / 2, + outer_rect.center().y() - size_inner.height() / 2); + return QRectF(top_left_inner, size_inner); + }; + + QRectF rect_inner = calculate_inner_rect(backgroundRect, 0.7, 0.7); + // Proportionally extend the rect to the right to account for visual icon distance to the left + rect_inner.setWidth(rect_inner.width() * 1.08); + + // Calculate distance row rectangle + const QSizeF distance_row_size(rect_inner.width(), rect_inner.height() / 2); + const QPointF distance_row_top_left(rect_inner.topLeft()); + const QRectF distance_row_rect_outer(distance_row_top_left, distance_row_size); + + // Calculate time row rectangle + const QSizeF time_row_size(rect_inner.width(), rect_inner.height() / 2); + const QPointF time_row_top_left( + rect_inner.topLeft().x(), rect_inner.topLeft().y() + rect_inner.height() / 2); + const QRectF time_row_rect_outer(time_row_top_left, time_row_size); + + const auto rect_time = calculate_inner_rect(time_row_rect_outer, 1.0, 0.9); + const auto rect_dist = calculate_inner_rect(distance_row_rect_outer, 1.0, 0.9); + + auto place_row = [&, this]( + const QRectF & rect, const QImage & icon, const QString & str_value, + const QString & str_unit) { + // order: icon, value, unit + + // place the icon + QPointF icon_top_left(rect.topLeft().x(), rect.center().y() - icon.height() / 2.0); + painter.drawImage(icon_top_left, icon); + + // place the unit text + const float unit_width = 40.0; + QRectF rect_text_unit( + rect.topRight().x() - unit_width, rect.topRight().y(), unit_width, rect.height()); + + painter.setPen(text_color); + painter.drawText(rect_text_unit, Qt::AlignLeft | Qt::AlignVCenter, str_unit); + + // place the value text + const double margin_x = 5.0; // margin around the text + + const double used_width = icon.width() + rect_text_unit.width() + margin_x * 2.0; + + QRectF rect_text( + rect.topLeft().x() + icon.width() + margin_x, rect.topLeft().y(), rect.width() - used_width, + rect.height()); + + painter.drawText(rect_text, Qt::AlignRight | Qt::AlignVCenter, str_value); + }; + + // remaining_time_ is in seconds + if (remaining_time_ <= 60) { + place_row(rect_time, icon_time_scaled_, QString::number(remaining_time_, 'f', 0), "sec"); + } else if (remaining_time_ <= 600) { + place_row(rect_time, icon_time_scaled_, QString::number(remaining_time_ / 60.0, 'f', 1), "min"); + } else if (remaining_time_ <= 3600) { + place_row(rect_time, icon_time_scaled_, QString::number(remaining_time_ / 60.0, 'f', 0), "min"); + } else if (remaining_time_ <= 36000) { + place_row( + rect_time, icon_time_scaled_, QString::number(remaining_time_ / 3600.0, 'f', 1), "hrs"); + } else { + place_row( + rect_time, icon_time_scaled_, QString::number(remaining_time_ / 3600.0, 'f', 0), "hrs"); + } + + // remaining_distance_ is in meters + if (remaining_distance_ <= 10) { + place_row(rect_dist, icon_dist_scaled_, QString::number(remaining_distance_, 'f', 1), "m"); + } else if (remaining_distance_ <= 1000) { + place_row(rect_dist, icon_dist_scaled_, QString::number(remaining_distance_, 'f', 0), "m"); + } else if (remaining_distance_ <= 10000) { + place_row( + rect_dist, icon_dist_scaled_, QString::number(remaining_distance_ / 1000.0, 'f', 2), "km"); + } else if (remaining_distance_ <= 100000) { + place_row( + rect_dist, icon_dist_scaled_, QString::number(remaining_distance_ / 1000.0, 'f', 1), "km"); + } else { + place_row( + rect_dist, icon_dist_scaled_, QString::number(remaining_distance_ / 1000.0, 'f', 0), "km"); + } +} + +} // namespace autoware::mission_details_overlay_rviz_plugin diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000..f8b88c9 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt @@ -0,0 +1,117 @@ +cmake_minimum_required(VERSION 3.8) +project(autoware_overlay_rviz_plugin) + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +set(headers_to_moc + 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_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 + autoware_vehicle_msgs + autoware_internal_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 "autoware_overlay_rviz_plugin-extras.cmake" +) diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/LICENSE b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/LICENSE new file mode 100644 index 0000000..a2fe2d3 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/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/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md new file mode 100644 index 0000000..f9a81b1 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md @@ -0,0 +1,54 @@ +# autoware_overlay_rviz_plugin + +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_vehicle_msgs::msg::VelocityReport` | The topic is vehicle velocity | +| `/vehicle/status/turn_indicators_status` | `autoware_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | +| `/vehicle/status/hazard_status` | `autoware_vehicle_msgs::msg::HazardReport` | The topic is status of hazard | +| `/vehicle/status/steering_status` | `autoware_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | +| `/vehicle/status/gear_status` | `autoware_vehicle_msgs::msg::GearReport` | The topic is status of gear | +| `/planning/scenario_planning/current_max_velocity` | `autoware_internal_planning_msgs::msg::VelocityLimit` | The topic is velocity limit | +| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficLightGroupArray` | The topic is status of traffic light | + +## 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 `rviz2` and click `Add` button under the `Displays` panel. + + ![select_add](./assets/images/select_add.png) + +2. Under `By display type` tab, select `autoware_overlay_rviz_plugin/SignalDisplay` and press OK. + +3. Enter the names of the topics if necessary. + + ![select_topic_name](./assets/images/select_topic_name.png) diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/LICENSE b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/LICENSE new file mode 100644 index 0000000..cc04d39 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/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/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Bold.ttf b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Bold.ttf new file mode 100644 index 0000000..07d5127 Binary files /dev/null and b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Bold.ttf differ diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Light.ttf b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Light.ttf new file mode 100644 index 0000000..8005310 Binary files /dev/null and b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Light.ttf differ diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Medium.ttf b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Medium.ttf new file mode 100644 index 0000000..f4634cd Binary files /dev/null and b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Medium.ttf differ diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Regular.ttf b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Regular.ttf new file mode 100644 index 0000000..60323ed Binary files /dev/null and b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Regular.ttf differ diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-SemiBold.ttf b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-SemiBold.ttf new file mode 100644 index 0000000..52059c3 Binary files /dev/null and b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-SemiBold.ttf differ diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/arrow.png b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/arrow.png new file mode 100644 index 0000000..18de535 Binary files /dev/null and b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/arrow.png differ diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/select_add.png b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/select_add.png new file mode 100644 index 0000000..c5130b6 Binary files /dev/null and b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/select_add.png differ diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/select_topic_name.png b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/select_topic_name.png new file mode 100644 index 0000000..490552e Binary files /dev/null and b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/select_topic_name.png differ diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/traffic.png b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/traffic.png new file mode 100644 index 0000000..79715b0 Binary files /dev/null and b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/traffic.png differ diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/wheel.png b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/wheel.png new file mode 100644 index 0000000..04ea298 Binary files /dev/null and b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/wheel.png differ diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin-extras.cmake b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin-extras.cmake new file mode 100644 index 0000000..9d6f156 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin-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/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp new file mode 100644 index 0000000..057cce9 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp @@ -0,0 +1,51 @@ +// 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_vehicle_msgs/msg/gear_report.hpp" + +#include +#include +#include + +namespace autoware_overlay_rviz_plugin +{ + +class GearDisplay +{ +public: + GearDisplay(); + void drawGearIndicator( + QPainter & painter, const QRectF & backgroundRect, const QColor & color, + const QColor & bg_color); + void updateGearData(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); + +private: + int current_gear_; // Internal variable to store current gear + QColor gray = QColor(194, 194, 194); +}; + +} // namespace autoware_overlay_rviz_plugin + +#endif // GEAR_DISPLAY_HPP_ diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/overlay_utils.hpp b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/overlay_utils.hpp new file mode 100644 index 0000000..b9c0608 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/overlay_utils.hpp @@ -0,0 +1,131 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace autoware_overlay_rviz_plugin +{ +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, TOP, BOTTOM }; + +enum class HorizontalAlignment : uint8_t { LEFT, RIGHT, 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 autoware_overlay_rviz_plugin + +#endif // OVERLAY_UTILS_HPP_ diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp new file mode 100644 index 0000000..c890cf3 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp @@ -0,0 +1,137 @@ +// 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 +#include +#endif + +namespace autoware_overlay_rviz_plugin +{ +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 updateTurnSignalBlinkingMode(); + 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_; + autoware_overlay_rviz_plugin::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_; + rviz_common::properties::FloatProperty * property_handle_angle_scale_; + rviz_common::properties::ColorProperty * property_background_color_; + rviz_common::properties::FloatProperty * property_background_alpha_; + rviz_common::properties::ColorProperty * property_primary_color_; + rviz_common::properties::ColorProperty * property_light_limit_color_; + rviz_common::properties::ColorProperty * property_dark_limit_color_; + + rviz_common::properties::EnumProperty * property_turn_signal_blinking_mode_; + + 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 drawHorizontalRoundedRectangle(QPainter & painter, const QRectF & backgroundRect); + void drawVerticalRoundedRectangle(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_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); + void updateSteeringData(const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); + void updateSpeedData(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + void updateTurnSignalsData( + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); + void updateHazardLightsData( + const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); + void updateSpeedLimitData( + const autoware_internal_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); + void updateTrafficLightData( + const autoware_perception_msgs::msg::TrafficLightGroup::ConstSharedPtr msg); + void drawWidget(QImage & hud); +}; +} // namespace autoware_overlay_rviz_plugin + +#endif // SIGNAL_DISPLAY_HPP_ diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp new file mode 100644 index 0000000..82d6ccc --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp @@ -0,0 +1,48 @@ +// 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_vehicle_msgs/msg/velocity_report.hpp" + +#include +#include +#include + +namespace autoware_overlay_rviz_plugin +{ + +class SpeedDisplay +{ +public: + SpeedDisplay(); + void drawSpeedDisplay(QPainter & painter, const QRectF & backgroundRect, const QColor & color); + void updateSpeedData(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + +private: + float current_speed_; // Internal variable to store current speed +}; + +} // namespace autoware_overlay_rviz_plugin + +#endif // SPEED_DISPLAY_HPP_ diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp new file mode 100644 index 0000000..6aa78b7 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp @@ -0,0 +1,55 @@ +// 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 "autoware_vehicle_msgs/msg/velocity_report.hpp" +#include + +#include +#include +#include + +namespace autoware_overlay_rviz_plugin +{ + +class SpeedLimitDisplay +{ +public: + SpeedLimitDisplay(); + void drawSpeedLimitIndicator( + QPainter & painter, const QRectF & backgroundRect, const QColor & color, + const QColor & light_color, const QColor & dark_color, const QColor & bg_color, + const float bg_alpha); + void updateSpeedLimitData( + const autoware_internal_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); + void updateSpeedData(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + +private: + float current_limit; // Internal variable to store current gear + float current_speed_; // Internal variable to store current speed +}; + +} // namespace autoware_overlay_rviz_plugin + +#endif // SPEED_LIMIT_DISPLAY_HPP_ diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp new file mode 100644 index 0000000..0e42368 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp @@ -0,0 +1,55 @@ +// 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_vehicle_msgs/msg/steering_report.hpp" + +#include +#include +#include + +namespace autoware_overlay_rviz_plugin +{ + +class SteeringWheelDisplay +{ +public: + SteeringWheelDisplay(); + void drawSteeringWheel( + QPainter & painter, const QRectF & backgroundRect, float handle_angle_scale_); + void updateSteeringData(const autoware_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); + autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr last_msg_ptr_; +}; + +} // namespace autoware_overlay_rviz_plugin + +#endif // STEERING_WHEEL_DISPLAY_HPP_ diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp new file mode 100644 index 0000000..01545bc --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp @@ -0,0 +1,58 @@ +// 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 + +namespace autoware_overlay_rviz_plugin +{ + +class TrafficDisplay +{ +public: + TrafficDisplay(); + void drawTrafficLightIndicator(QPainter & painter, const QRectF & backgroundRect); + void updateTrafficLightData( + const autoware_perception_msgs::msg::TrafficLightGroup::ConstSharedPtr & msg); + autoware_perception_msgs::msg::TrafficLightGroup current_traffic_; + +private: + QImage traffic_light_image_; + + const QColor tl_red_; + const QColor tl_yellow_; + const QColor tl_green_; + const QColor tl_gray_; + + QImage coloredImage(const QImage & source, const QColor & color); +}; + +} // namespace autoware_overlay_rviz_plugin + +#endif // TRAFFIC_DISPLAY_HPP_ diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp new file mode 100644 index 0000000..2d94908 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/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 +#include + +namespace autoware_overlay_rviz_plugin +{ + +class TurnSignalsDisplay +{ +public: + TurnSignalsDisplay(); + void drawArrows(QPainter & painter, const QRectF & backgroundRect, const QColor & color); + void updateTurnSignalsData( + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); + void updateHazardLightsData( + const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); + void setBlinkingMode(std::string_view mode); + +private: + QImage arrowImage; + QColor gray = QColor(79, 79, 79); + + std::string blinking_mode_ = "Static"; + + 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); +}; + +} // namespace autoware_overlay_rviz_plugin + +#endif // TURN_SIGNALS_DISPLAY_HPP_ diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml new file mode 100644 index 0000000..8576bfe --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml @@ -0,0 +1,31 @@ + + + + autoware_overlay_rviz_plugin + 0.0.0 + + 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 + + ament_index_cpp + autoware_internal_planning_msgs + autoware_perception_msgs + autoware_vehicle_msgs + boost + rviz_common + rviz_ogre_vendor + rviz_rendering + + ament_lint_auto + autoware_lint_common + + ament_cmake_auto + + + ament_cmake + + diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/plugins_description.xml b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/plugins_description.xml new file mode 100644 index 0000000..05c33f2 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/plugins_description.xml @@ -0,0 +1,5 @@ + + + Signal overlay plugin for the 3D view. + + diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp new file mode 100644 index 0000000..12e25ee --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp @@ -0,0 +1,101 @@ +// 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 +#include + +namespace autoware_overlay_rviz_plugin +{ + +GearDisplay::GearDisplay() : current_gear_(0) +{ + std::string package_path = + ament_index_cpp::get_package_share_directory("autoware_overlay_rviz_plugin"); + 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_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, const QColor & color, const QColor & bg_color) +{ + // we deal with the different gears here + std::string gearString; + switch (current_gear_) { + case autoware_vehicle_msgs::msg::GearReport::NEUTRAL: + gearString = "N"; + break; + case autoware_vehicle_msgs::msg::GearReport::LOW: + case autoware_vehicle_msgs::msg::GearReport::LOW_2: + gearString = "L"; + break; + case autoware_vehicle_msgs::msg::GearReport::NONE: + case autoware_vehicle_msgs::msg::GearReport::PARK: + gearString = "P"; + break; + case autoware_vehicle_msgs::msg::GearReport::REVERSE: + case autoware_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", 12, QFont::Bold); + painter.setFont(gearFont); + QPen borderPen(gray); + borderPen.setWidth(1); + painter.setPen(borderPen); + + double gearBoxSize = 37.5; + double gearX = backgroundRect.left() + 54; + double gearY = backgroundRect.height() / 2 - gearBoxSize / 2; + QRect gearRect(gearX, gearY, gearBoxSize, gearBoxSize); + painter.setBrush(color); + painter.drawRoundedRect(gearRect, 10, 10); + painter.setPen(bg_color); + painter.drawText(gearRect, Qt::AlignCenter, QString::fromStdString(gearString)); +} + +} // namespace autoware_overlay_rviz_plugin diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/overlay_utils.cpp b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/overlay_utils.cpp new file mode 100644 index 0000000..076b92a --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/overlay_utils.cpp @@ -0,0 +1,269 @@ +// 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 + +#include + +namespace autoware_overlay_rviz_plugin +{ +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 autoware_overlay_rviz_plugin diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp new file mode 100644 index 0000000..6fbd925 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp @@ -0,0 +1,514 @@ +// 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 +#include + +namespace autoware_overlay_rviz_plugin +{ + +SignalDisplay::SignalDisplay() +{ + property_width_ = new rviz_common::properties::IntProperty( + "Width", 550, "Width of the overlay", this, SLOT(updateOverlaySize())); + property_height_ = new rviz_common::properties::IntProperty( + "Height", 100, "Height of the overlay", this, SLOT(updateOverlaySize())); + property_left_ = new rviz_common::properties::IntProperty( + "Left", 0, "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(QString("#00E678")), "Color of the signal arrows", this, + SLOT(updateOverlayColor())); + property_handle_angle_scale_ = new rviz_common::properties::FloatProperty( + "Handle Angle Scale", 17.0, "Scale of the steering wheel handle angle", this, + SLOT(updateOverlaySize())); + property_background_color_ = new rviz_common::properties::ColorProperty( + "Background Color", QColor(0, 0, 0), "Color of the signal arrows", this, + SLOT(updateOverlayColor())); + property_background_alpha_ = new rviz_common::properties::FloatProperty( + "Background Alpha", 0.3, "Background Color Alpha", this, SLOT(updateOverlayColor())); + property_primary_color_ = new rviz_common::properties::ColorProperty( + "Primary Color", QColor(174, 174, 174), "Color of the signal arrows", this, + SLOT(updateOverlayColor())); + property_light_limit_color_ = new rviz_common::properties::ColorProperty( + "Light Traffic Color", QColor(255, 153, 153), "Color of the signal arrows", this, + SLOT(updateOverlayColor())); + property_dark_limit_color_ = new rviz_common::properties::ColorProperty( + "Dark Traffic Color", QColor(255, 51, 51), "Color of the signal arrows", this, + SLOT(updateOverlayColor())); + property_turn_signal_blinking_mode_ = new rviz_common::properties::EnumProperty( + "Signal Blinking Mode", "Static", "State of the signal blinking", this, + SLOT(updateTurnSignalBlinkingMode())); + property_turn_signal_blinking_mode_->addOption("Static", 0); + property_turn_signal_blinking_mode_->addOption("Blinking", 1); + + // 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 autoware_overlay_rviz_plugin::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_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_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_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_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_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", + "autoware_internal_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", + "/planning/scenario_planning/lane_driving/behavior_planning/debug/traffic_signal", + "autoware_perception_msgs/msgs/msg/TrafficLightGroup", "Topic for Traffic Light Data", this, + SLOT(topic_updated_traffic())); + traffic_topic_property_->initialize(rviz_ros_node); +} + +void SignalDisplay::setupRosSubscriptions() +{ + topic_updated_gear(); + topic_updated_steering(); + topic_updated_speed(); + topic_updated_speed_limit(); + topic_updated_turn_signals(); + topic_updated_hazard_lights(); + topic_updated_traffic(); +} + +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; + } + autoware_overlay_rviz_plugin::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::TrafficLightGroup::ConstSharedPtr msg) +{ + std::lock_guard lock(property_mutex_); + + if (traffic_display_) { + traffic_display_->updateTrafficLightData(msg); + } +} + +void SignalDisplay::updateSpeedLimitData( + const autoware_internal_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_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_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_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_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) +{ + std::lock_guard lock(property_mutex_); + + if (speed_display_) { + speed_display_->updateSpeedData(msg); + speed_limit_display_->updateSpeedData(msg); + queueRender(); + } +} + +void SignalDisplay::updateTurnSignalsData( + const autoware_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, hud.width(), hud.height()); + drawHorizontalRoundedRectangle(painter, backgroundRect); + + // Draw components + if (gear_display_) { + gear_display_->drawGearIndicator( + painter, backgroundRect, property_primary_color_->getColor(), + property_background_color_->getColor()); + } + + if (steering_wheel_display_) { + steering_wheel_display_->drawSteeringWheel( + painter, backgroundRect, property_handle_angle_scale_->getFloat()); + } + + if (speed_display_) { + speed_display_->drawSpeedDisplay(painter, backgroundRect, property_primary_color_->getColor()); + } + if (turn_signals_display_) { + turn_signals_display_->drawArrows(painter, backgroundRect, property_signal_color_->getColor()); + } + + if (traffic_display_) { + traffic_display_->drawTrafficLightIndicator(painter, backgroundRect); + } + + if (speed_limit_display_) { + speed_limit_display_->drawSpeedLimitIndicator( + painter, backgroundRect, property_primary_color_->getColor(), + property_light_limit_color_->getColor(), property_dark_limit_color_->getColor(), + property_background_color_->getColor(), property_background_alpha_->getFloat()); + } + + painter.end(); +} + +void SignalDisplay::drawHorizontalRoundedRectangle( + QPainter & painter, const QRectF & backgroundRect) +{ + painter.setRenderHint(QPainter::Antialiasing, true); + QColor colorFromHSV; + colorFromHSV.setHsv( + property_background_color_->getColor().hue(), + property_background_color_->getColor().saturation(), + property_background_color_->getColor().value()); + colorFromHSV.setAlphaF(property_background_alpha_->getFloat()); + painter.setBrush(colorFromHSV); + + painter.setPen(Qt::NoPen); + painter.drawRoundedRect( + backgroundRect, backgroundRect.height() / 2, backgroundRect.height() / 2); // Circular ends +} +void SignalDisplay::drawVerticalRoundedRectangle(QPainter & painter, const QRectF & backgroundRect) +{ + painter.setRenderHint(QPainter::Antialiasing, true); + QColor colorFromHSV; + colorFromHSV.setHsv( + property_background_color_->getColor().hue(), + property_background_color_->getColor().saturation(), + property_background_color_->getColor().value()); + colorFromHSV.setAlphaF(property_background_alpha_->getFloat()); + + painter.setBrush(colorFromHSV); + + painter.setPen(Qt::NoPen); + painter.drawRoundedRect( + backgroundRect, backgroundRect.width() / 2, backgroundRect.width() / 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(), HorizontalAlignment::CENTER, + VerticalAlignment::TOP); + queueRender(); +} + +void SignalDisplay::updateOverlayColor() +{ + std::lock_guard lock(mutex_); + queueRender(); +} + +void SignalDisplay::updateTurnSignalBlinkingMode() +{ + std::lock_guard lock(mutex_); + if (turn_signals_display_) { + turn_signals_display_->setBlinkingMode(property_turn_signal_blinking_mode_->getStdString()); + } + 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_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_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_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)).transient_local(), + [this](const autoware_internal_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_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_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::TrafficLightGroup::SharedPtr msg) { + updateTrafficLightData(msg); + }); +} + +} // namespace autoware_overlay_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS(autoware_overlay_rviz_plugin::SignalDisplay, rviz_common::Display) diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp new file mode 100644 index 0000000..83d7f2f --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp @@ -0,0 +1,115 @@ +// 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 +#include + +namespace autoware_overlay_rviz_plugin +{ + +SpeedDisplay::SpeedDisplay() : current_speed_(0.0) +{ + std::string package_path = + ament_index_cpp::get_package_share_directory("autoware_overlay_rviz_plugin"); + 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_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_ = std::abs(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_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, const QColor & color) +{ + QFont referenceFont("Quicksand", 80, QFont::Bold); + painter.setFont(referenceFont); + QRect referenceRect = painter.fontMetrics().boundingRect("88"); + QPointF referencePos( + backgroundRect.width() / 2.0 - referenceRect.width() / 2.0 - 5.0, + backgroundRect.height() / 2.0); + + QString speedNumber = QString::number(current_speed_, 'f', 0); + int fontSize = 40; + 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.0, + backgroundRect.center().y() + speedNumberRect.bottom()); + painter.setPen(color); + painter.drawText(speedPos, speedNumber); + + QFont unitFont("Quicksand", 8, QFont::DemiBold); + painter.setFont(unitFont); + QString speedUnit = "km/h"; + QRect unitRect = painter.fontMetrics().boundingRect(speedUnit); + QPointF unitPos( + (backgroundRect.width() / 2.0 - unitRect.width() / 2.0), + referencePos.y() + unitRect.height() + 15.0); + painter.drawText(unitPos, speedUnit); +} + +} // namespace autoware_overlay_rviz_plugin diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp new file mode 100644 index 0000000..eeeaf2b --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp @@ -0,0 +1,147 @@ +// 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 +#include +#include + +namespace autoware_overlay_rviz_plugin +{ + +SpeedLimitDisplay::SpeedLimitDisplay() : current_limit(0.0), current_speed_(0.0) +{ + std::string package_path = + ament_index_cpp::get_package_share_directory("autoware_overlay_rviz_plugin"); + 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 autoware_internal_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) +{ + current_limit = msg->max_velocity; +} + +void SpeedLimitDisplay::updateSpeedData( + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) +{ + try { + float speed = msg->longitudinal_velocity; + current_speed_ = speed; + } catch (const std::exception & e) { + // Log the error + std::cerr << "Error in processMessage: " << e.what() << std::endl; + } +} + +void SpeedLimitDisplay::drawSpeedLimitIndicator( + QPainter & painter, const QRectF & backgroundRect, const QColor & color, + const QColor & light_color, const QColor & dark_color, const QColor & bg_color, + const float bg_alpha) +{ + // Enable Antialiasing for smoother drawing + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setRenderHint(QPainter::SmoothPixmapTransform, true); + + QColor borderColor = light_color; + + if (current_limit > 0.0) { + double speed_to_limit_ratio = current_speed_ / current_limit; + const double speed_to_limit_ratio_min = 0.6; + const double speed_to_limit_ratio_max = 0.9; + + if (speed_to_limit_ratio >= speed_to_limit_ratio_max) { + borderColor = dark_color; + } else if (speed_to_limit_ratio > speed_to_limit_ratio_min) { + double interpolation_factor = (speed_to_limit_ratio - speed_to_limit_ratio_min) / + (speed_to_limit_ratio_max - speed_to_limit_ratio_min); + // Interpolate between light_color and dark_color + int red = light_color.red() + (dark_color.red() - light_color.red()) * interpolation_factor; + int green = + light_color.green() + (dark_color.green() - light_color.green()) * interpolation_factor; + int blue = + light_color.blue() + (dark_color.blue() - light_color.blue()) * interpolation_factor; + borderColor = QColor(red, green, blue); + } + } + + // Define the area for the outer circle + QRectF outerCircleRect = QRectF(45, 45, 45, 45); + outerCircleRect.moveTopRight(QPointF( + backgroundRect.right() - 44, backgroundRect.height() / 2 - outerCircleRect.height() / 2)); + + // Now use borderColor for drawing + painter.setPen(QPen(borderColor, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + painter.setBrush(borderColor); + // Draw the outer circle for the speed limit indicator + painter.drawEllipse(outerCircleRect); + + // Define the area for the inner circle + QRectF innerCircleRect = outerCircleRect; + innerCircleRect.setWidth(outerCircleRect.width() / 1.09); + innerCircleRect.setHeight(outerCircleRect.height() / 1.09); + innerCircleRect.moveCenter(outerCircleRect.center()); + + QRectF innerCircleRect2 = innerCircleRect; + + painter.setRenderHint(QPainter::Antialiasing, true); + QColor colorFromHSV; + colorFromHSV.setHsv(bg_color.hue(), bg_color.saturation(), bg_color.value()); + painter.setBrush(colorFromHSV); + painter.drawEllipse(innerCircleRect); + + // Add a second inner circle as a mask to make the speed limit indicator look like a ring + // and follow the rest of the background color as close as possible + painter.drawEllipse(innerCircleRect2); + + 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", 16, QFont::Bold); + + painter.setFont(font); + painter.setPen(QPen(color, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + + // Draw the text in the center of the circle + painter.drawText(innerCircleRect, Qt::AlignCenter, text); +} + +} // namespace autoware_overlay_rviz_plugin diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp new file mode 100644 index 0000000..fec3ecf --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp @@ -0,0 +1,147 @@ +// 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 +#include + +namespace autoware_overlay_rviz_plugin +{ + +SteeringWheelDisplay::SteeringWheelDisplay() +{ + // Load the Quicksand font + std::string package_path = + ament_index_cpp::get_package_share_directory("autoware_overlay_rviz_plugin"); + 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_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) +{ + try { + last_msg_ptr_ = msg; + + steering_angle_ = msg->steering_tire_angle; + } 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, float handle_angle_scale_) +{ + // 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 = std::round(handle_angle_scale_ * (steering_angle_ / M_PI) * -180.0); + // Calculate the position + int wheelCenterX = backgroundRect.left() + wheel.width() + 54 + 54; + int wheelCenterY = backgroundRect.height() / 2; + + // 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.0, wheelCenterY - rotatedWheel.height() / 2.0); + + // Draw the rotated image + painter.drawImage(drawPoint.x(), drawPoint.y(), rotatedWheel); + + std::ostringstream steering_angle_ss; + if (last_msg_ptr_) { + steering_angle_ss << std::fixed << std::setprecision(1) << steering_angle_ * 180.0 / M_PI + << "°"; + } else { + steering_angle_ss << "N/A"; + } + + QString steeringAngleString = QString::fromStdString(steering_angle_ss.str()); + // if the string doesn't have a negative sign, add a space to the front of the string + // to align the text in both cases (negative and positive) + if (steeringAngleString[0] != '-') { + steeringAngleString = " " + steeringAngleString; + } + + // Draw the steering angle text + QFont steeringFont("Quicksand", 8, QFont::Bold); + painter.setFont(steeringFont); + painter.setPen(QColor(0, 0, 0, 255)); + // QRect steeringRect( + // wheelCenterX - wheelImage.width() / 2 + 1, wheelCenterY - wheelImage.height() / 2, + // wheelImage.width(), wheelImage.height()); + // painter.drawText(steeringRect, Qt::AlignCenter, steeringAngleString); + + // Measure the text + QFontMetrics fontMetrics(steeringFont); + QRect textRect = fontMetrics.boundingRect(steeringAngleString); + + // Center the text + int textX = wheelCenterX - textRect.width() / 2; + int textY = wheelCenterY - textRect.height() / 2; + + QRect steeringRect(textX, textY, textRect.width(), textRect.height()); + painter.drawText(steeringRect, Qt::AlignCenter, steeringAngleString); +} + +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 autoware_overlay_rviz_plugin diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp new file mode 100644 index 0000000..817d498 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_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 "traffic_display.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware_overlay_rviz_plugin +{ + +TrafficDisplay::TrafficDisplay() +: tl_red_(QString("#cc3d3d")), + tl_yellow_(QString("#ccb43d")), + tl_green_(QString("#3dcc55")), + tl_gray_(QString("#4f4f4f")) +{ + // Load the traffic light image + std::string package_path = + ament_index_cpp::get_package_share_directory("autoware_overlay_rviz_plugin"); + 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::TrafficLightGroup::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); + + painter.setBrush(QBrush(tl_gray_, Qt::SolidPattern)); + painter.setPen(Qt::NoPen); + // Define the area for the circle (background) + QRectF circleRect = QRectF(50, 50, 50, 50); + circleRect.moveTopRight(QPointF( + backgroundRect.right() - circleRect.width() - 75, + backgroundRect.height() / 2 - circleRect.height() / 2)); + painter.drawEllipse(circleRect); + + if (!current_traffic_.elements.empty()) { + switch (current_traffic_.elements[0].color) { + case 1: + painter.setBrush(QBrush(tl_red_)); + painter.drawEllipse(circleRect); + break; + case 2: + painter.setBrush(QBrush(tl_yellow_)); + painter.drawEllipse(circleRect); + break; + case 3: + painter.setBrush(QBrush(tl_green_)); + painter.drawEllipse(circleRect); + break; + case 4: + default: + painter.setBrush(tl_gray_); + painter.drawEllipse(circleRect); + break; + } + } + + // Scaling factor (e.g., 1.5 for 150% size) + float scaleFactor = 0.75; + + // Calculate the scaled size + QSize scaledSize = traffic_light_image_.size() * scaleFactor; + + // Ensure the scaled image is still within the circle bounds or adjust scaleFactor accordingly + + // Calculate the centered rectangle for the scaled image + QRectF scaledImageRect(0, 0, scaledSize.width(), scaledSize.height()); + scaledImageRect.moveCenter(circleRect.center()); + + // Scale the image to the new size + QImage scaledTrafficImage = + traffic_light_image_.scaled(scaledSize, Qt::KeepAspectRatio, Qt::SmoothTransformation); + + // Draw the scaled and centered image + painter.drawImage(scaledImageRect.topLeft(), scaledTrafficImage); +} + +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 autoware_overlay_rviz_plugin diff --git a/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp new file mode 100644 index 0000000..bbcaae2 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp @@ -0,0 +1,120 @@ +// 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 +#include +#include + +namespace autoware_overlay_rviz_plugin +{ + +TurnSignalsDisplay::TurnSignalsDisplay() : current_turn_signal_(0) +{ + // Load the arrow image + std::string package_path = + ament_index_cpp::get_package_share_directory("autoware_overlay_rviz_plugin"); + std::string image_path = package_path + "/assets/images/arrow.png"; + arrowImage.load(image_path.c_str()); +} + +void TurnSignalsDisplay::updateTurnSignalsData( + const autoware_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_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::setBlinkingMode(std::string_view mode) +{ + blinking_mode_ = mode; +} + +void TurnSignalsDisplay::drawArrows( + QPainter & painter, const QRectF & backgroundRect, const QColor & color) +{ + QImage scaledLeftArrow = arrowImage.scaled(50, 32, Qt::KeepAspectRatio, Qt::SmoothTransformation); + scaledLeftArrow = coloredImage(scaledLeftArrow, gray); + QImage scaledRightArrow = scaledLeftArrow.mirrored(true, false); + int arrowYPos = (backgroundRect.height() / 2.0 - scaledLeftArrow.height() / 2.0 - 4.0); + int leftArrowXPos = backgroundRect.left() + scaledLeftArrow.width() * 2 + 180; + int rightArrowXPos = backgroundRect.right() - scaledRightArrow.width() * 3 - 175; + + bool leftActive = + (current_turn_signal_ == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT || + current_hazard_lights_ == autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE); + bool rightActive = + (current_turn_signal_ == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT || + current_hazard_lights_ == autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE); + + auto now = std::chrono::steady_clock::now().time_since_epoch(); + auto elapsed = std::chrono::duration_cast(now).count(); + + bool blink_off = blinking_mode_ == "Blinking" && (elapsed / 333) % 2 == 0; + if (leftActive && !blink_off) { + scaledLeftArrow = coloredImage(scaledLeftArrow, color); + } + if (rightActive && !blink_off) { + scaledRightArrow = coloredImage(scaledRightArrow, color); + } + + // Draw the arrows + painter.drawImage(QPointF(leftArrowXPos, arrowYPos), scaledLeftArrow); + painter.drawImage(QPointF(rightArrowXPos, arrowYPos), scaledRightArrow); +} + +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 autoware_overlay_rviz_plugin diff --git a/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/CMakeLists.txt b/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000..7ff4e19 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.8) +project(autoware_string_stamped_rviz_plugin) + +# find dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugins_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + plugins +) diff --git a/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/README.md b/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/README.md new file mode 100644 index 0000000..e5add26 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/README.md @@ -0,0 +1,7 @@ +# autoware_string_stamped_rviz_plugin + +Plugin for displaying 2D overlays over the RViz2 3D scene. + +## Purpose + +This plugin displays the ROS message whose topic type is `autoware_internal_debug_msgs::msg::StringStamped` in rviz. diff --git a/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/package.xml b/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/package.xml new file mode 100644 index 0000000..52567cf --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/package.xml @@ -0,0 +1,30 @@ + + + + autoware_string_stamped_rviz_plugin + 0.0.0 + + 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). + + Takayuki Murooka + Satoshi Ota + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + ament_index_cpp + autoware_internal_debug_msgs + rviz_common + rviz_ogre_vendor + rviz_rendering + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/plugins/plugins_description.xml b/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/plugins/plugins_description.xml new file mode 100644 index 0000000..302bcc6 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/plugins/plugins_description.xml @@ -0,0 +1,5 @@ + + + String stamped overlay plugin for the 3D view. + + diff --git a/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.cpp b/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.cpp new file mode 100644 index 0000000..03fd8bc --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.cpp @@ -0,0 +1,225 @@ +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// 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 {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 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.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "jsk_overlay_utils.hpp" + +#include + +namespace jsk_rviz_plugins +{ +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( + Ogre::SceneManager * manager, rclcpp::Logger logger, const std::string & name) +: name_(name), logger_(logger) +{ + rviz_rendering::RenderSystem::get()->prepareOverlays(manager); + 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() +{ + hide(); + panel_material_->unload(); + Ogre::MaterialManager::getSingleton().remove(panel_material_->getName()); + // Ogre::OverlayManager* mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + // mOverlayMgr->destroyOverlayElement(panel_); + // delete panel_; + // delete overlay_; +} + +std::string OverlayObject::getName() +{ + return name_; +} + +void OverlayObject::hide() +{ + if (overlay_->isVisible()) { + overlay_->hide(); + } +} + +void OverlayObject::show() +{ + if (!overlay_->isVisible()) { + overlay_->show(); + } +} + +bool OverlayObject::isTextureReady() +{ + return static_cast(texture_); +} + +void OverlayObject::updateTextureSize(unsigned int width, unsigned int height) +{ + const std::string texture_name = name_ + "Texture"; + if (width == 0) { + RCLCPP_WARN(logger_, "width=0 is specified as texture size"); + width = 1; + } + if (height == 0) { + RCLCPP_WARN(logger_, "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 left, double top) +{ + panel_->setPosition(left, top); +} + +void OverlayObject::setDimensions(double width, double height) +{ + panel_->setDimensions(width, height); +} + +bool OverlayObject::isVisible() +{ + return overlay_->isVisible(); +} + +unsigned int OverlayObject::getTextureWidth() +{ + if (isTextureReady()) { + return texture_->getWidth(); + } else { + return 0; + } +} + +unsigned int OverlayObject::getTextureHeight() +{ + if (isTextureReady()) { + return texture_->getHeight(); + } else { + return 0; + } +} + +} // namespace jsk_rviz_plugins diff --git a/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.hpp b/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.hpp new file mode 100644 index 0000000..e69abed --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.hpp @@ -0,0 +1,143 @@ +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// 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 {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 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.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef JSK_OVERLAY_UTILS_HPP_ +#define JSK_OVERLAY_UTILS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +// see OGRE/OgrePrerequisites.h +// #define OGRE_VERSION +// ((OGRE_VERSION_MAJOR << 16) | (OGRE_VERSION_MINOR << 8) | OGRE_VERSION_PATCH) +#if OGRE_VERSION < ((1 << 16) | (9 << 8) | 0) +#include +#include +#include +#include +#else +#include +#include +#include +#include +#include +#endif + +#include +#include +#include +#include +#include +#include + +namespace jsk_rviz_plugins +{ +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_; + +private: +}; + +// this is a class for put overlay object on rviz 3D panel. +// This class suppose to be instantiated in onInitialize method +// of rviz::Display class. +class OverlayObject +{ +public: + typedef std::shared_ptr Ptr; + + OverlayObject(Ogre::SceneManager * manager, rclcpp::Logger logger, const std::string & name); + virtual ~OverlayObject(); + + virtual std::string getName(); + /*virtual*/ void hide(); // remove "virtual" for cppcheck + virtual void show(); + virtual bool isTextureReady(); + virtual void updateTextureSize(unsigned int width, unsigned int height); + virtual ScopedPixelBuffer getBuffer(); + virtual void setPosition(double left, double top); + virtual void setDimensions(double width, double height); + virtual bool isVisible(); + virtual unsigned int getTextureWidth(); + virtual unsigned int getTextureHeight(); + +protected: + const std::string name_; + rclcpp::Logger logger_; + Ogre::Overlay * overlay_; + Ogre::PanelOverlayElement * panel_; + Ogre::MaterialPtr panel_material_; + Ogre::TexturePtr texture_; + +private: +}; + +// Ogre::Overlay* createOverlay(std::string name); +// Ogre::PanelOverlayElement* createOverlayPanel(Ogre::Overlay* overlay); +// Ogre::MaterialPtr createOverlayMaterial(Ogre::Overlay* overlay); +} // namespace jsk_rviz_plugins + +#endif // JSK_OVERLAY_UTILS_HPP_ diff --git a/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.cpp b/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.cpp new file mode 100644 index 0000000..d5746e9 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.cpp @@ -0,0 +1,237 @@ +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// 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 {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 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.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "string_stamped_overlay_display.hpp" + +#include "jsk_overlay_utils.hpp" + +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace autoware::string_stamped_rviz_plugin +{ +StringStampedOverlayDisplay::StringStampedOverlayDisplay() +{ + const Screen * screen_info = DefaultScreenOfDisplay(XOpenDisplay(NULL)); + + constexpr float hight_4k = 2160.0; + const float scale = static_cast(screen_info->height) / hight_4k; + const auto left = static_cast(std::round(1024 * scale)); + const auto top = static_cast(std::round(128 * scale)); + + property_text_color_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(25, 255, 240), "text color", this, SLOT(updateVisualization()), this); + property_left_ = new rviz_common::properties::IntProperty( + "Left", left, "Left of the plotter window", this, SLOT(updateVisualization()), this); + property_left_->setMin(0); + property_top_ = new rviz_common::properties::IntProperty( + "Top", top, "Top of the plotter window", this, SLOT(updateVisualization())); + property_top_->setMin(0); + + property_value_height_offset_ = new rviz_common::properties::IntProperty( + "Value height offset", 0, "Height offset of the plotter window", this, + SLOT(updateVisualization())); + property_font_size_ = new rviz_common::properties::IntProperty( + "Font Size", 15, "Font Size", this, SLOT(updateVisualization()), this); + property_font_size_->setMin(1); + property_max_letter_num_ = new rviz_common::properties::IntProperty( + "Max Letter Num", 100, "Max Letter Num", this, SLOT(updateVisualization()), this); + property_max_letter_num_->setMin(10); + + property_last_diag_keep_time_ = new rviz_common::properties::FloatProperty( + "Time To Keep Last Diag", 1.0, "Time To Keep Last Diag", this, SLOT(updateVisualization()), + this); + property_last_diag_keep_time_->setMin(0); + + property_last_diag_erase_time_ = new rviz_common::properties::FloatProperty( + "Time To Erase Last Diag", 2.0, "Time To Erase Last Diag", this, SLOT(updateVisualization()), + this); + property_last_diag_erase_time_->setMin(0.001); +} + +StringStampedOverlayDisplay::~StringStampedOverlayDisplay() +{ + if (initialized()) { + overlay_->hide(); + } +} + +void StringStampedOverlayDisplay::onInitialize() +{ + RTDClass::onInitialize(); + + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "StringOverlayDisplayObject" << count++; + auto logger = context_->getRosNodeAbstraction().lock()->get_raw_node()->get_logger(); + overlay_.reset(new jsk_rviz_plugins::OverlayObject(scene_manager_, logger, ss.str())); + + overlay_->show(); + + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +void StringStampedOverlayDisplay::onEnable() +{ + subscribe(); + overlay_->show(); +} + +void StringStampedOverlayDisplay::onDisable() +{ + unsubscribe(); + reset(); + overlay_->hide(); +} + +void StringStampedOverlayDisplay::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + { + std::lock_guard message_lock(mutex_); + if (!last_non_empty_msg_ptr_) { + return; + } + } + + // calculate text and alpha + const auto text_with_alpha = [&]() { + std::lock_guard message_lock(mutex_); + if (last_msg_text_.empty()) { + const auto current_time = context_->getRosNodeAbstraction().lock()->get_raw_node()->now(); + const auto duration = (current_time - last_non_empty_msg_ptr_->stamp).seconds(); + if ( + duration < + property_last_diag_keep_time_->getFloat() + property_last_diag_erase_time_->getFloat()) { + const int dynamic_alpha = static_cast(std::max( + (1.0 - std::max(duration - property_last_diag_keep_time_->getFloat(), 0.0) / + property_last_diag_erase_time_->getFloat()) * + 255, + 0.0)); + return std::make_pair(last_non_empty_msg_ptr_->data, dynamic_alpha); + } + } + return std::make_pair(last_msg_text_, 255); + }(); + + // Display + QColor background_color; + background_color.setAlpha(0); + jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(background_color); + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + const int w = overlay_->getTextureWidth() - line_width_; + const int h = overlay_->getTextureHeight() - line_width_; + + // text + QColor text_color(property_text_color_->getColor()); + text_color.setAlpha(text_with_alpha.second); + painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); + QFont font = painter.font(); + font.setPixelSize(property_font_size_->getInt()); + font.setBold(true); + painter.setFont(font); + + // same as above, but align on right side + painter.drawText( + 0, std::min(property_value_height_offset_->getInt(), h - 1), w, + std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignLeft | Qt::AlignTop, + text_with_alpha.first.c_str()); + painter.end(); + updateVisualization(); +} + +void StringStampedOverlayDisplay::processMessage( + const autoware_internal_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) +{ + if (!isEnabled()) { + return; + } + + { + std::lock_guard message_lock(mutex_); + last_msg_text_ = msg_ptr->data; + + // keep the non empty last message for visualization + if (!msg_ptr->data.empty()) { + last_non_empty_msg_ptr_ = msg_ptr; + } + } + + queueRender(); +} + +void StringStampedOverlayDisplay::updateVisualization() +{ + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +} // namespace autoware::string_stamped_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::string_stamped_rviz_plugin::StringStampedOverlayDisplay, rviz_common::Display) diff --git a/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.hpp b/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.hpp new file mode 100644 index 0000000..6f7cd84 --- /dev/null +++ b/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.hpp @@ -0,0 +1,110 @@ +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// 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 {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 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.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef STRING_STAMPED_OVERLAY_DISPLAY_HPP_ +#define STRING_STAMPED_OVERLAY_DISPLAY_HPP_ + +#include +#include +#include + +#ifndef Q_MOC_RUN +#include "jsk_overlay_utils.hpp" + +#include +#include +#include +#include + +#endif + +#include + +namespace autoware::string_stamped_rviz_plugin +{ +class StringStampedOverlayDisplay +: public rviz_common::RosTopicDisplay + +{ + Q_OBJECT + +public: + StringStampedOverlayDisplay(); + ~StringStampedOverlayDisplay() override; + + void onInitialize() override; + void onEnable() override; + void onDisable() override; + +private Q_SLOTS: + void updateVisualization(); + +protected: + void update(float wall_dt, float ros_dt) override; + void processMessage( + const autoware_internal_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) override; + jsk_rviz_plugins::OverlayObject::Ptr overlay_; + rviz_common::properties::ColorProperty * property_text_color_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::IntProperty * property_value_height_offset_; + rviz_common::properties::IntProperty * property_font_size_; + rviz_common::properties::IntProperty * property_max_letter_num_; + rviz_common::properties::FloatProperty * property_last_diag_keep_time_; + rviz_common::properties::FloatProperty * property_last_diag_erase_time_; + +private: + static constexpr int line_width_ = 2; + static constexpr int hand_width_ = 4; + + std::mutex mutex_; + std::string last_msg_text_; + autoware_internal_debug_msgs::msg::StringStamped::ConstSharedPtr last_non_empty_msg_ptr_; +}; +} // namespace autoware::string_stamped_rviz_plugin + +#endif // STRING_STAMPED_OVERLAY_DISPLAY_HPP_