diff --git a/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/CMakeLists.txt b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..8692ea1bb152a --- /dev/null +++ b/common/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/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/LICENSE b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/LICENSE new file mode 100644 index 0000000000000..a2fe2d3d1c7ed --- /dev/null +++ b/common/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/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/README.md b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/README.md new file mode 100644 index 0000000000000..3a4040a7065d0 --- /dev/null +++ b/common/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/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/av_timer.png b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/av_timer.png new file mode 100644 index 0000000000000..f7f5db1249511 Binary files /dev/null and b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/av_timer.png differ diff --git a/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/font/Quicksand/LICENSE b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/font/Quicksand/LICENSE new file mode 100644 index 0000000000000..cc04d393573f0 --- /dev/null +++ b/common/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/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Bold.ttf b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Bold.ttf new file mode 100644 index 0000000000000..07d5127c04b17 Binary files /dev/null and b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Bold.ttf differ diff --git a/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Regular.ttf b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Regular.ttf new file mode 100644 index 0000000000000..60323ed6abdf1 Binary files /dev/null and b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/font/Quicksand/static/Quicksand-Regular.ttf differ diff --git a/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/path.png b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/path.png new file mode 100644 index 0000000000000..62af338b8c053 Binary files /dev/null and b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/assets/path.png differ diff --git a/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin-extras.cmake b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin-extras.cmake new file mode 100644 index 0000000000000..9d6f156555e57 --- /dev/null +++ b/common/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/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/mission_details_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/mission_details_display.hpp new file mode 100644 index 0000000000000..ab5abfe3c3d83 --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/mission_details_display.hpp @@ -0,0 +1,83 @@ +// 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_; + 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/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/overlay_utils.hpp b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/overlay_utils.hpp new file mode 100644 index 0000000000000..a0344f872f524 --- /dev/null +++ b/common/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/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/remaining_distance_time_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/remaining_distance_time_display.hpp new file mode 100644 index 0000000000000..843b75e352648 --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/include/remaining_distance_time_display.hpp @@ -0,0 +1,57 @@ +// 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); + 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 + + QColor gray = QColor(194, 194, 194); + + 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/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/package.xml b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/package.xml new file mode 100644 index 0000000000000..1054ac4f516bd --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/package.xml @@ -0,0 +1,28 @@ + + + + autoware_mission_details_overlay_rviz_plugin + 0.0.1 + + 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/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/plugins_description.xml b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/plugins_description.xml new file mode 100644 index 0000000000000..5177b10859704 --- /dev/null +++ b/common/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/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/mission_details_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/mission_details_display.cpp new file mode 100644 index 0000000000000..bafae1727b2f1 --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/mission_details_display.cpp @@ -0,0 +1,212 @@ +// 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())); + + // 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); + } + + painter.end(); +} + +void MissionDetailsDisplay::draw_rounded_rect(QPainter & painter, const QRectF & backgroundRect) +{ + painter.setRenderHint(QPainter::Antialiasing, true); + QColor colorFromHSV; + colorFromHSV.setHsv(0, 0, 29); + colorFromHSV.setAlphaF(0.60); + + 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/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/overlay_utils.cpp b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/overlay_utils.cpp new file mode 100644 index 0000000000000..68813a5f1140f --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/overlay_utils.cpp @@ -0,0 +1,267 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// -*- mode: c++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Team Spatzenhirn + * Copyright (c) 2014, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "overlay_utils.hpp" + +#include + +namespace 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/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/remaining_distance_time_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/remaining_distance_time_display.cpp new file mode 100644 index 0000000000000..86395ef1918bc --- /dev/null +++ b/common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/src/remaining_distance_time_display.cpp @@ -0,0 +1,157 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "remaining_distance_time_display.hpp" + +#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 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(gray); + 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/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index 710fb20631a45..0e693943a4d03 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -43,5 +43,10 @@ + + + + + diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 950ef67865a85..d04a405a61c7b 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -57,6 +57,7 @@ ament_cmake_auto autoware_cmake + autoware_remaining_distance_time_calculator behavior_path_planner behavior_velocity_planner costmap_generator diff --git a/planning/autoware_remaining_distance_time_calculator/CMakeLists.txt b/planning/autoware_remaining_distance_time_calculator/CMakeLists.txt new file mode 100644 index 0000000000000..d29a153a0ce5d --- /dev/null +++ b/planning/autoware_remaining_distance_time_calculator/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_remaining_distance_time_calculator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/remaining_distance_time_calculator_node.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::remaining_distance_time_calculator::RemainingDistanceTimeCalculatorNode" + EXECUTABLE ${PROJECT_NAME}_node +) + + +ament_auto_package( + INSTALL_TO_SHARE + config + launch +) diff --git a/planning/autoware_remaining_distance_time_calculator/README.md b/planning/autoware_remaining_distance_time_calculator/README.md new file mode 100644 index 0000000000000..694c6764de91c --- /dev/null +++ b/planning/autoware_remaining_distance_time_calculator/README.md @@ -0,0 +1,39 @@ +## Remaining Distance and Time Calculator + +### Role + +This package aims to provide mission remaining distance and remaining time calculations. + +### Activation and Timing + +- The calculations are activated once we have a route planned for a mission in Autoware. +- The calculations are triggered timely based on the `update_rate` parameter. + +### Module Parameters + +| Name | Type | Default Value | Explanation | +| ------------- | ------ | ------------- | --------------------------- | +| `update_rate` | double | 10.0 | Timer callback period. [Hz] | + +### Inner-workings + +#### Remaining Distance Calculation + +- The remaining distance calculation is based on getting the remaining shortest path between the current vehicle pose and goal pose using `lanelet2` routing APIs. +- The remaining distance is calculated by summing the 2D length of remaining shortest path, with exception to current lanelet and goal lanelet. + - For the current lanelet, the distance is calculated from the current vehicle position to the end of that lanelet. + - For the goal lanelet, the distance is calculated from the start of the lanelet to the goal pose in this lanelet. +- When there is only one lanelet remaining, the distance is calculated by getting the 2D distance between the current vehicle pose and goal pose. +- Checks are added to handle cases when current lanelet, goal lanelet, or routing graph are not valid to prevent node process die. + - In such cases when, last valid remaining distance and time are maintained. + +#### Remaining Time Calculation + +- The remaining time currently depends on a simple equation of motion by getting the maximum velocity limit. +- The remaining distance is calculated by dividing the remaining distance by the maximum velocity limit. +- A check is added to the remaining time calculation to make sure that maximum velocity limit is greater than zero. This prevents division by zero or getting negative time value. + +### Future Work + +- Find a more efficient way for remaining distance calculation instead of regularly searching the graph for finding the remaining shortest path. +- Engage more sophisticated motion models for more accurate remaining time calculations. diff --git a/planning/autoware_remaining_distance_time_calculator/config/remaining_distance_time_calculator.param.yaml b/planning/autoware_remaining_distance_time_calculator/config/remaining_distance_time_calculator.param.yaml new file mode 100644 index 0000000000000..5b8c019de5a20 --- /dev/null +++ b/planning/autoware_remaining_distance_time_calculator/config/remaining_distance_time_calculator.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + update_rate: 10.0 diff --git a/planning/autoware_remaining_distance_time_calculator/launch/remaining_distance_time_calculator.launch.xml b/planning/autoware_remaining_distance_time_calculator/launch/remaining_distance_time_calculator.launch.xml new file mode 100644 index 0000000000000..cfb456c57ca41 --- /dev/null +++ b/planning/autoware_remaining_distance_time_calculator/launch/remaining_distance_time_calculator.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/planning/autoware_remaining_distance_time_calculator/package.xml b/planning/autoware_remaining_distance_time_calculator/package.xml new file mode 100644 index 0000000000000..4f0324b23f299 --- /dev/null +++ b/planning/autoware_remaining_distance_time_calculator/package.xml @@ -0,0 +1,35 @@ + + + autoware_remaining_distance_time_calculator + 0.1.0 + Calculates and publishes remaining distance and time of the mission. + + Ahmed Ebrahim + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_internal_msgs + autoware_planning_msgs + geometry_msgs + lanelet2_extension + motion_utils + nav_msgs + planning_test_utils + rclcpp + rclcpp_components + route_handler + std_msgs + tier4_autoware_utils + tier4_planning_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/autoware_remaining_distance_time_calculator/schema/remaining_distance_time_calculator.schema.json b/planning/autoware_remaining_distance_time_calculator/schema/remaining_distance_time_calculator.schema.json new file mode 100644 index 0000000000000..4b3bc15432d0d --- /dev/null +++ b/planning/autoware_remaining_distance_time_calculator/schema/remaining_distance_time_calculator.schema.json @@ -0,0 +1,31 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Remaining Distance Time Calculator Parameters", + "type": "object", + "actual_parameters": { + "type": "object", + "properties": { + "update_rate": { + "type": "number", + "default": 10.0, + "description": "Recalculate remaining distance and time at this rate, Hz" + } + }, + "required": ["update_rate"], + "additionalProperties": false + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/actual_parameters" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp new file mode 100644 index 0000000000000..327bd0ff3b582 --- /dev/null +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp @@ -0,0 +1,188 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "remaining_distance_time_calculator_node.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace autoware::remaining_distance_time_calculator +{ + +RemainingDistanceTimeCalculatorNode::RemainingDistanceTimeCalculatorNode( + const rclcpp::NodeOptions & options) +: Node("remaining_distance_time_calculator", options), + is_graph_ready_{false}, + has_received_route_{false}, + velocity_limit_{99.99}, + remaining_distance_{0.0}, + remaining_time_{0.0} +{ + using std::placeholders::_1; + + sub_odometry_ = create_subscription( + "~/input/odometry", 1, std::bind(&RemainingDistanceTimeCalculatorNode::on_odometry, this, _1)); + + const auto qos_transient_local = rclcpp::QoS{1}.transient_local(); + + sub_map_ = create_subscription( + "~/input/map", qos_transient_local, + std::bind(&RemainingDistanceTimeCalculatorNode::on_map, this, _1)); + sub_route_ = create_subscription( + "~/input/route", qos_transient_local, + std::bind(&RemainingDistanceTimeCalculatorNode::on_route, this, _1)); + sub_planning_velocity_ = create_subscription( + "/planning/scenario_planning/current_max_velocity", qos_transient_local, + std::bind(&RemainingDistanceTimeCalculatorNode::on_velocity_limit, this, _1)); + + pub_mission_remaining_distance_time_ = create_publisher( + "~/output/mission_remaining_distance_time", + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable()); + + node_param_.update_rate = declare_parameter("update_rate"); + + const auto period_ns = rclcpp::Rate(node_param_.update_rate).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&RemainingDistanceTimeCalculatorNode::on_timer, this)); +} + +void RemainingDistanceTimeCalculatorNode::on_map(const HADMapBin::ConstSharedPtr & msg) +{ + route_handler_.setMap(*msg); + lanelet_map_ptr_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); + road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets); + is_graph_ready_ = true; +} + +void RemainingDistanceTimeCalculatorNode::on_odometry(const Odometry::ConstSharedPtr & msg) +{ + current_vehicle_pose_ = msg->pose.pose; + current_vehicle_velocity_ = msg->twist.twist.linear; +} + +void RemainingDistanceTimeCalculatorNode::on_route(const LaneletRoute::ConstSharedPtr & msg) +{ + goal_pose_ = msg->goal_pose; + has_received_route_ = true; +} + +void RemainingDistanceTimeCalculatorNode::on_velocity_limit( + const VelocityLimit::ConstSharedPtr & msg) +{ + if (msg->max_velocity > 1e-5) { + velocity_limit_ = msg->max_velocity; + } +} + +void RemainingDistanceTimeCalculatorNode::on_timer() +{ + if (is_graph_ready_ && has_received_route_) { + calculate_remaining_distance(); + calculate_remaining_time(); + publish_mission_remaining_distance_time(); + } +} + +void RemainingDistanceTimeCalculatorNode::calculate_remaining_distance() +{ + size_t index = 0; + + lanelet::ConstLanelet current_lanelet; + if (!lanelet::utils::query::getClosestLanelet( + road_lanelets_, current_vehicle_pose_, ¤t_lanelet)) { + RCLCPP_WARN_STREAM(this->get_logger(), "Failed to find current lanelet."); + + return; + } + + lanelet::ConstLanelet goal_lanelet; + if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal_pose_, &goal_lanelet)) { + RCLCPP_WARN_STREAM(this->get_logger(), "Failed to find goal lanelet."); + + return; + } + + const lanelet::Optional optional_route = + routing_graph_ptr_->getRoute(current_lanelet, goal_lanelet, 0); + if (!optional_route) { + RCLCPP_WARN_STREAM(this->get_logger(), "Failed to find proper route."); + + return; + } + + lanelet::routing::LaneletPath remaining_shortest_path; + remaining_shortest_path = optional_route->shortestPath(); + + remaining_distance_ = 0.0; + + for (auto & llt : remaining_shortest_path) { + if (remaining_shortest_path.size() == 1) { + remaining_distance_ += + tier4_autoware_utils::calcDistance2d(current_vehicle_pose_.position, goal_pose_.position); + break; + } + + if (index == 0) { + lanelet::ArcCoordinates arc_coord = + lanelet::utils::getArcCoordinates({llt}, current_vehicle_pose_); + double this_lanelet_length = lanelet::utils::getLaneletLength2d(llt); + + remaining_distance_ += this_lanelet_length - arc_coord.length; + } else if (index == (remaining_shortest_path.size() - 1)) { + lanelet::ArcCoordinates arc_coord = lanelet::utils::getArcCoordinates({llt}, goal_pose_); + remaining_distance_ += arc_coord.length; + } else { + remaining_distance_ += lanelet::utils::getLaneletLength2d(llt); + } + + index++; + } +} + +void RemainingDistanceTimeCalculatorNode::calculate_remaining_time() +{ + if (velocity_limit_ > 0.0) { + remaining_time_ = remaining_distance_ / velocity_limit_; + } +} + +void RemainingDistanceTimeCalculatorNode::publish_mission_remaining_distance_time() +{ + MissionRemainingDistanceTime mission_remaining_distance_time; + + mission_remaining_distance_time.remaining_distance = remaining_distance_; + mission_remaining_distance_time.remaining_time = remaining_time_; + pub_mission_remaining_distance_time_->publish(mission_remaining_distance_time); +} + +} // namespace autoware::remaining_distance_time_calculator + +#include +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::remaining_distance_time_calculator::RemainingDistanceTimeCalculatorNode) diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp new file mode 100644 index 0000000000000..2a88cdb57abf4 --- /dev/null +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp @@ -0,0 +1,110 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef REMAINING_DISTANCE_TIME_CALCULATOR_NODE_HPP_ +#define REMAINING_DISTANCE_TIME_CALCULATOR_NODE_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +namespace autoware::remaining_distance_time_calculator +{ + +struct NodeParam +{ + double update_rate{0.0}; +}; + +class RemainingDistanceTimeCalculatorNode : public rclcpp::Node +{ +public: + explicit RemainingDistanceTimeCalculatorNode(const rclcpp::NodeOptions & options); + +private: + using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute; + using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using Odometry = nav_msgs::msg::Odometry; + using VelocityLimit = tier4_planning_msgs::msg::VelocityLimit; + using MissionRemainingDistanceTime = autoware_internal_msgs::msg::MissionRemainingDistanceTime; + + rclcpp::Subscription::SharedPtr sub_route_; + rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_odometry_; + rclcpp::Subscription::SharedPtr sub_planning_velocity_; + + rclcpp::Publisher::SharedPtr pub_mission_remaining_distance_time_; + + rclcpp::TimerBase::SharedPtr timer_; + + route_handler::RouteHandler route_handler_; + lanelet::LaneletMapPtr lanelet_map_ptr_; + lanelet::routing::RoutingGraphPtr routing_graph_ptr_; + lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_; + lanelet::ConstLanelets road_lanelets_; + bool is_graph_ready_; + + // Data Buffer + geometry_msgs::msg::Pose current_vehicle_pose_; + geometry_msgs::msg::Vector3 current_vehicle_velocity_; + geometry_msgs::msg::Pose goal_pose_; + bool has_received_route_; + double velocity_limit_; + + double remaining_distance_; + double remaining_time_; + + // Parameter + NodeParam node_param_; + + // Callbacks + void on_timer(); + void on_odometry(const Odometry::ConstSharedPtr & msg); + void on_route(const LaneletRoute::ConstSharedPtr & msg); + void on_map(const HADMapBin::ConstSharedPtr & msg); + void on_velocity_limit(const VelocityLimit::ConstSharedPtr & msg); + + /** + * @brief calculate mission remaining distance + */ + void calculate_remaining_distance(); + + /** + * @brief calculate mission remaining time + */ + void calculate_remaining_time(); + + /** + * @brief publish mission remaining distance and time + */ + void publish_mission_remaining_distance_time(); +}; +} // namespace autoware::remaining_distance_time_calculator +#endif // REMAINING_DISTANCE_TIME_CALCULATOR_NODE_HPP_