feat(pointcloud_preprocessor): support 3d distortion corrector for distortion corrector node

.github/PULL_REQUEST_TEMPLATE/small-change.md

@@ -15,6 +15,10 @@ Not applicable.
 
 Not applicable.
 
+## Interface changes
+
+<!-- Describe any changed interfaces, such as topics, services, or parameters, including debugging interfaces -->
+
 ## Pre-review checklist for the PR author
 
 The PR author **must** check the checkboxes below when creating the PR.

.github/PULL_REQUEST_TEMPLATE/standard-change.md

@@ -18,6 +18,19 @@
 
 <!-- Describe any changed interfaces, such as topics, services, or parameters. -->
 
+### ROS Topic Changes
+
+<!-- | Topic Name       | Type                | Direction | Update Description                                            | -->
+<!-- | ---------------- | ------------------- | --------- | ------------------------------------------------------------- | -->
+<!-- | `/example_topic` | `std_msgs/String`   | Subscribe | Description of what the topic is used for in the system       | -->
+<!-- | `/another_topic` | `sensor_msgs/Image` | Publish   | Also explain if it is added / modified / deleted with the PR | -->
+
+### ROS Parameter Changes
+
+<!-- | Parameter Name       | Default Value | Update Description                                  | -->
+<!-- | -------------------- | ------------- | --------------------------------------------------- | -->
+<!-- | `example_parameters` | `1.0`         | Describe the parameter and also explain the updates | -->
+
 ## Effects on system behavior
 
 <!-- Describe how this PR affects the system behavior. -->

build_depends.repos

@@ -41,3 +41,7 @@ repositories:
     type: git
     url: https://github.com/MORAI-Autonomous/MORAI-ROS2_morai_msgs.git
     version: main
+  universe/external/glog:  # TODO: to use isGoogleInitialized() API in v0.6.0. Remove when the rosdep glog version is updated to v0.6.0 (already updated in Ubuntu 24.04)
+    type: git
+    url: https://github.com/tier4/glog.git
+    version: v0.6.0_t4-ros

common/autoware_auto_common/include/autoware_auto_common/common/types.hpp

@@ -37,7 +37,9 @@ namespace types
 // We don't currently require code to comply to MISRA, but we should try to where it is
 // easily possible.
 using bool8_t = bool;
+#if __cplusplus < 201811L || !__cpp_char8_t
 using char8_t = char;
+#endif
 using uchar8_t = unsigned char;
 // If we ever compile on a platform where this is not true, float32_t and float64_t definitions
 // need to be adjusted.

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
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+  ${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"
+)

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.

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
+
+- <https://fonts.google.com/icons?selected=Material+Symbols+Outlined:conversion_path:FILL@1;wght@400;GRAD@200;opsz@20&icon.size=20&icon.color=%23e8eaed&icon.query=path>
+- <https://fonts.google.com/icons?selected=Material+Symbols+Outlined:av_timer:FILL@1;wght@400;GRAD@200;opsz@20&icon.size=20&icon.color=%23e8eaed&icon.query=av+timer>

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.

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)

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 <QImage>
+#include <QString>
+#include <rviz_common/display.hpp>
+#include <rviz_common/properties/color_property.hpp>
+#include <rviz_common/properties/float_property.hpp>
+#include <rviz_common/properties/int_property.hpp>
+#include <rviz_common/properties/ros_topic_property.hpp>
+
+#include <OgreColourValue.h>
+#include <OgreMaterial.h>
+#include <OgreTexture.h>
+
+#include <memory>
+#include <mutex>
+#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<rviz_common::properties::RosTopicProperty>
+    remaining_distance_time_topic_property_;
+
+  void draw_rounded_rect(QPainter & painter, const QRectF & backgroundRect);
+  void setupRosSubscriptions();
+
+  std::unique_ptr<RemainingDistanceTimeDisplay> remaining_distance_time_display_;
+
+  rclcpp::Subscription<autoware_internal_msgs::msg::MissionRemainingDistanceTime>::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_

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 <QColor>
+#include <QImage>
+
+#include <OgreHardwarePixelBuffer.h>
+#include <OgreMaterialManager.h>
+#include <OgreTechnique.h>
+#include <OgreTexture.h>
+#include <OgreTextureManager.h>
+#include <Overlay/OgreOverlay.h>
+#include <Overlay/OgreOverlayContainer.h>
+#include <Overlay/OgreOverlayElement.h>
+#include <Overlay/OgreOverlayManager.h>
+#include <Overlay/OgrePanelOverlayElement.h>
+
+#include <memory>
+#include <string>
+
+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<OverlayObject>;
+
+  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_

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 <QImage>
+#include <QString>
+#include <rviz_common/properties/color_property.hpp>
+#include <rviz_common/properties/float_property.hpp>
+#include <rviz_common/properties/int_property.hpp>
+#include <rviz_common/ros_topic_display.hpp>
+
+#include <autoware_internal_msgs/msg/mission_remaining_distance_time.hpp>
+
+#include <OgreColourValue.h>
+#include <OgreMaterial.h>
+#include <OgreTexture.h>
+
+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_

common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_mission_details_overlay_rviz_plugin</name>
+  <version>0.0.1</version>
+  <description>
+    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).
+  </description>
+  <maintainer email="ahmed.ebrahim@leodrive.ai">Ahmed Ebrahim</maintainer>
+
+  <license>BSD-3-Clause</license>
+
+  <depend>autoware_internal_msgs</depend>
+  <depend>boost</depend>
+  <depend>rviz_common</depend>
+  <depend>rviz_ogre_vendor</depend>
+  <depend>rviz_rendering</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/plugins_description.xml

@@ -0,0 +1,5 @@
+<library path="autoware_mission_details_overlay_rviz_plugin">
+  <class name="autoware_mission_details_overlay_rviz_plugin/MissionDetailsDisplay" type="autoware::mission_details_overlay_rviz_plugin::MissionDetailsDisplay" base_class_type="rviz_common::Display">
+    <description>Mission Details overlay plugin for the 3D view.</description>
+  </class>
+</library>

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 <QFontDatabase>
+#include <QPainter>
+#include <rclcpp/rclcpp.hpp>
+#include <rviz_common/properties/ros_topic_property.hpp>
+#include <rviz_rendering/render_system.hpp>
+
+#include <algorithm>
+#include <iomanip>
+#include <memory>
+#include <mutex>
+#include <string>
+
+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<RemainingDistanceTimeDisplay>();
+}
+
+void MissionDetailsDisplay::onInitialize()
+{
+  std::lock_guard<std::mutex> 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<autoware::mission_details_overlay_rviz_plugin::OverlayObject>(ss.str());
+  overlay_->show();
+  update_size();
+
+  auto rviz_ros_node = context_->getRosNodeAbstraction();
+
+  remaining_distance_time_topic_property_ =
+    std::make_unique<rviz_common::properties::RosTopicProperty>(
+      "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<autoware_internal_msgs::msg::MissionRemainingDistanceTime>(
+      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<std::mutex> 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<std::mutex> lock(property_mutex_);
+  if (overlay_) {
+    overlay_->show();
+  }
+  setupRosSubscriptions();
+}
+
+void MissionDetailsDisplay::onDisable()
+{
+  std::lock_guard<std::mutex> 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<std::mutex> lock(property_mutex_);
+
+  if (remaining_distance_time_display_) {
+    remaining_distance_time_display_->updateRemainingDistanceTimeData(msg);
+    queueRender();
+  }
+}
+
+void MissionDetailsDisplay::draw_widget(QImage & hud)
+{
+  std::lock_guard<std::mutex> 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<std::mutex> 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<autoware_internal_msgs::msg::MissionRemainingDistanceTime>(
+        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/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(
+  autoware::mission_details_overlay_rviz_plugin::MissionDetailsDisplay, rviz_common::Display)

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 <rviz_common/logging.hpp>
+
+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<Ogre::uint8 *>(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<Ogre::PanelOverlayElement *>(
+    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

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 <QColor>
+#include <QFontDatabase>
+#include <QPainter>
+#include <ament_index_cpp/get_package_share_directory.hpp>
+
+#include <qpoint.h>
+
+#include <cmath>
+#include <iomanip>
+#include <memory>
+#include <string>
+
+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

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt

@@ -3,28 +3,13 @@ project(autoware_overlay_rviz_plugin)
 
 # find dependencies
 find_package(ament_cmake_auto REQUIRED)
-find_package(ament_cmake REQUIRED)
-find_package(autoware_auto_vehicle_msgs REQUIRED)
-find_package(tier4_planning_msgs REQUIRED)
-find_package(autoware_perception_msgs REQUIRED)
 ament_auto_find_build_dependencies()
 
-find_package(autoware_overlay_msgs REQUIRED)
-
-find_package(rviz_common REQUIRED)
-find_package(rviz_rendering REQUIRED)
-find_package(rviz_ogre_vendor REQUIRED)
-find_package(std_msgs REQUIRED)
-
-set(
-        headers_to_moc
-        include/overlay_text_display.hpp
-        include/signal_display.hpp
-
+set(headers_to_moc
+  include/signal_display.hpp
 )
 
-set(
-  headers_to_not_moc
+set(headers_to_not_moc
   include/steering_wheel_display.hpp
   include/gear_display.hpp
   include/speed_display.hpp
@@ -34,47 +19,42 @@ set(
 )
 
 
-
 foreach(header "${headers_to_moc}")
-    qt5_wrap_cpp(display_moc_files "${header}")
+  qt5_wrap_cpp(display_moc_files "${header}")
 endforeach()
 
-set(
-        display_source_files
-        src/overlay_text_display.cpp
-        src/overlay_utils.cpp
-        src/signal_display.cpp
-        src/steering_wheel_display.cpp
-        src/gear_display.cpp
-        src/speed_display.cpp
-        src/turn_signals_display.cpp
-        src/traffic_display.cpp
-        src/speed_limit_display.cpp
-
+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}
+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
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>
-        ${Qt5Widgets_INCLUDE_DIRS}
-        ${OpenCV_INCLUDE_DIRS}
+  ${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+  ${Qt5Widgets_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
 )
 
 target_link_libraries(
-        ${PROJECT_NAME} PUBLIC
-        rviz_ogre_vendor::OgreMain
-        rviz_ogre_vendor::OgreOverlay
+  ${PROJECT_NAME} PUBLIC
+  rviz_ogre_vendor::OgreMain
+  rviz_ogre_vendor::OgreOverlay
 )
 
 
@@ -85,22 +65,19 @@ target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNC
 
 pluginlib_export_plugin_description_file(rviz_common plugins_description.xml)
 
-ament_target_dependencies(
-        ${PROJECT_NAME}
-        PUBLIC
-        rviz_common
-        rviz_rendering
-        autoware_overlay_msgs
-        autoware_auto_vehicle_msgs
-        tier4_planning_msgs
-        autoware_perception_msgs
+ament_target_dependencies(${PROJECT_NAME} PUBLIC
+  rviz_common
+  rviz_rendering
+  autoware_auto_vehicle_msgs
+  tier4_planning_msgs
+  autoware_perception_msgs
 )
 
 ament_export_include_directories(include)
 ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
 ament_export_dependencies(
-        rviz_common
-        rviz_ogre_vendor
+  rviz_common
+  rviz_ogre_vendor
 )
 
 if(BUILD_TESTING)
@@ -109,32 +86,32 @@ if(BUILD_TESTING)
 endif()
 
 install(
-        TARGETS ${PROJECT_NAME}
-        EXPORT ${PROJECT_NAME}
-        ARCHIVE DESTINATION lib
-        LIBRARY DESTINATION lib
-        RUNTIME DESTINATION bin
-        INCLUDES DESTINATION include
+  TARGETS ${PROJECT_NAME}
+  EXPORT ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
 )
 
 install(
-        DIRECTORY include/
-        DESTINATION include
+  DIRECTORY include/
+  DESTINATION include
 )
 
 install(
-        TARGETS
-        DESTINATION lib/${PROJECT_NAME}
+  TARGETS
+  DESTINATION lib/${PROJECT_NAME}
 )
 
 # Copy the assets directory to the installation directory
 install(
-    DIRECTORY assets/
-    DESTINATION share/${PROJECT_NAME}/assets
+  DIRECTORY assets/
+  DESTINATION share/${PROJECT_NAME}/assets
 )
 
 add_definitions(-DQT_NO_KEYWORDS)
 
 ament_package(
-        CONFIG_EXTRAS "autoware_overlay_rviz_plugin-extras.cmake"
+  CONFIG_EXTRAS "autoware_overlay_rviz_plugin-extras.cmake"
 )

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/overlay_utils.hpp

@@ -54,8 +54,6 @@
 #include <QColor>
 #include <QImage>
 
-#include "autoware_overlay_msgs/msg/overlay_text.hpp"
-
 #include <OgreHardwarePixelBuffer.h>
 #include <OgreMaterialManager.h>
 #include <OgreTechnique.h>
@@ -89,17 +87,9 @@ class ScopedPixelBuffer
   Ogre::HardwarePixelBufferSharedPtr pixel_buffer_;
 };
 
-enum class VerticalAlignment : uint8_t {
-  CENTER = autoware_overlay_msgs::msg::OverlayText::CENTER,
-  TOP = autoware_overlay_msgs::msg::OverlayText::TOP,
-  BOTTOM = autoware_overlay_msgs::msg::OverlayText::BOTTOM,
-};
+enum class VerticalAlignment : uint8_t { CENTER, TOP, BOTTOM };
 
-enum class HorizontalAlignment : uint8_t {
-  LEFT = autoware_overlay_msgs::msg::OverlayText::LEFT,
-  RIGHT = autoware_overlay_msgs::msg::OverlayText::RIGHT,
-  CENTER = autoware_overlay_msgs::msg::OverlayText::CENTER
-};
+enum class HorizontalAlignment : uint8_t { LEFT, RIGHT, CENTER };
 
 /**
  * Helper class for realizing an overlay object on top of the rviz 3D panel.

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp

@@ -47,7 +47,7 @@ class TurnSignalsDisplay
 
 private:
   QImage arrowImage;
-  QColor gray = QColor(46, 46, 46);
+  QColor gray = QColor(79, 79, 79);
 
   int current_turn_signal_;    // Internal variable to store turn signal state
   int current_hazard_lights_;  // Internal variable to store hazard lights state

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml

@@ -12,7 +12,6 @@
   <license>BSD-3-Clause</license>
 
   <depend>autoware_auto_vehicle_msgs</depend>
-  <depend>autoware_overlay_msgs</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>boost</depend>
   <depend>rviz_common</depend>

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp

@@ -81,18 +81,19 @@ void GearDisplay::drawGearIndicator(QPainter & painter, const QRectF & backgroun
       break;
   }
 
-  QFont gearFont("Quicksand", 16, QFont::Bold);
+  QFont gearFont("Quicksand", 12, QFont::Bold);
   painter.setFont(gearFont);
   QPen borderPen(gray);
-  borderPen.setWidth(4);
+  borderPen.setWidth(1);
   painter.setPen(borderPen);
 
-  int gearBoxSize = 30;
-  int gearX = backgroundRect.left() + 30 + gearBoxSize;
-  int gearY = backgroundRect.height() - gearBoxSize - 20;
+  double gearBoxSize = 37.5;
+  double gearX = backgroundRect.left() + 54;
+  double gearY = backgroundRect.height() / 2 - gearBoxSize / 2;
   QRect gearRect(gearX, gearY, gearBoxSize, gearBoxSize);
-  painter.setBrush(QColor(0, 0, 0, 0));
+  painter.setBrush(gray);
   painter.drawRoundedRect(gearRect, 10, 10);
+  painter.setPen(Qt::black);
   painter.drawText(gearRect, Qt::AlignCenter, QString::fromStdString(gearString));
 }
 

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp

@@ -39,11 +39,11 @@ namespace autoware_overlay_rviz_plugin
 SignalDisplay::SignalDisplay()
 {
   property_width_ = new rviz_common::properties::IntProperty(
-    "Width", 517, "Width of the overlay", this, SLOT(updateOverlaySize()));
+    "Width", 550, "Width of the overlay", this, SLOT(updateOverlaySize()));
   property_height_ = new rviz_common::properties::IntProperty(
-    "Height", 175, "Height of the overlay", this, SLOT(updateOverlaySize()));
+    "Height", 100, "Height of the overlay", this, SLOT(updateOverlaySize()));
   property_left_ = new rviz_common::properties::IntProperty(
-    "Left", 10, "Left position of the overlay", this, SLOT(updateOverlayPosition()));
+    "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(
@@ -120,59 +120,13 @@ void SignalDisplay::onInitialize()
 
 void SignalDisplay::setupRosSubscriptions()
 {
-  // Don't create a node, just use the one from the parent class
-  auto rviz_node_ = context_->getRosNodeAbstraction().lock()->get_raw_node();
-
-  gear_sub_ = rviz_node_->create_subscription<autoware_auto_vehicle_msgs::msg::GearReport>(
-    gear_topic_property_->getTopicStd(),
-    rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
-    [this](const autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg) {
-      updateGearData(msg);
-    });
-
-  steering_sub_ = rviz_node_->create_subscription<autoware_auto_vehicle_msgs::msg::SteeringReport>(
-    steering_topic_property_->getTopicStd(),
-    rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
-    [this](const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) {
-      updateSteeringData(msg);
-    });
-
-  speed_sub_ = rviz_node_->create_subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>(
-    speed_topic_property_->getTopicStd(),
-    rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
-    [this](const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg) {
-      updateSpeedData(msg);
-    });
-
-  turn_signals_sub_ =
-    rviz_node_->create_subscription<autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport>(
-      turn_signals_topic_property_->getTopicStd(),
-      rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
-      [this](const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::SharedPtr msg) {
-        updateTurnSignalsData(msg);
-      });
-
-  hazard_lights_sub_ =
-    rviz_node_->create_subscription<autoware_auto_vehicle_msgs::msg::HazardLightsReport>(
-      hazard_lights_topic_property_->getTopicStd(),
-      rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
-      [this](const autoware_auto_vehicle_msgs::msg::HazardLightsReport::SharedPtr msg) {
-        updateHazardLightsData(msg);
-      });
-
-  traffic_sub_ = rviz_node_->create_subscription<autoware_perception_msgs::msg::TrafficSignal>(
-    traffic_topic_property_->getTopicStd(),
-    rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
-    [this](const autoware_perception_msgs::msg::TrafficSignal::SharedPtr msg) {
-      updateTrafficLightData(msg);
-    });
-
-  speed_limit_sub_ = rviz_node_->create_subscription<tier4_planning_msgs::msg::VelocityLimit>(
-    speed_limit_topic_property_->getTopicStd(),
-    rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
-    [this](const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) {
-      updateSpeedLimitData(msg);
-    });
+  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()
@@ -325,35 +279,31 @@ void SignalDisplay::drawWidget(QImage & hud)
   QPainter painter(&hud);
   painter.setRenderHint(QPainter::Antialiasing, true);
 
-  QRectF backgroundRect(0, 0, 322, hud.height());
+  QRectF backgroundRect(0, 0, 550, hud.height());
   drawHorizontalRoundedRectangle(painter, backgroundRect);
 
   // Draw components
-  if (steering_wheel_display_) {
-    steering_wheel_display_->drawSteeringWheel(painter, backgroundRect);
-  }
   if (gear_display_) {
     gear_display_->drawGearIndicator(painter, backgroundRect);
   }
+
+  if (steering_wheel_display_) {
+    steering_wheel_display_->drawSteeringWheel(painter, backgroundRect);
+  }
+
   if (speed_display_) {
     speed_display_->drawSpeedDisplay(painter, backgroundRect);
   }
   if (turn_signals_display_) {
     turn_signals_display_->drawArrows(painter, backgroundRect, property_signal_color_->getColor());
   }
 
-  // a 27px space between the two halves of the HUD
-
-  QRectF smallerBackgroundRect(340, 0, 190.0 / 2, hud.height());
-
-  drawVerticalRoundedRectangle(painter, smallerBackgroundRect);
-
   if (traffic_display_) {
-    traffic_display_->drawTrafficLightIndicator(painter, smallerBackgroundRect);
+    traffic_display_->drawTrafficLightIndicator(painter, backgroundRect);
   }
 
   if (speed_limit_display_) {
-    speed_limit_display_->drawSpeedLimitIndicator(painter, smallerBackgroundRect);
+    speed_limit_display_->drawSpeedLimitIndicator(painter, backgroundRect);
   }
 
   painter.end();
@@ -364,8 +314,8 @@ void SignalDisplay::drawHorizontalRoundedRectangle(
 {
   painter.setRenderHint(QPainter::Antialiasing, true);
   QColor colorFromHSV;
-  colorFromHSV.setHsv(0, 0, 0);  // Hue, Saturation, Value
-  colorFromHSV.setAlphaF(0.65);  // Transparency
+  colorFromHSV.setHsv(0, 0, 29);  // Hue, Saturation, Value
+  colorFromHSV.setAlphaF(0.60);   // Transparency
 
   painter.setBrush(colorFromHSV);
 
@@ -404,7 +354,9 @@ void SignalDisplay::updateOverlaySize()
 void SignalDisplay::updateOverlayPosition()
 {
   std::lock_guard<std::mutex> lock(mutex_);
-  overlay_->setPosition(property_left_->getInt(), property_top_->getInt());
+  overlay_->setPosition(
+    property_left_->getInt(), property_top_->getInt(), HorizontalAlignment::CENTER,
+    VerticalAlignment::TOP);
   queueRender();
 }
 
@@ -421,8 +373,7 @@ void SignalDisplay::topic_updated_gear()
   auto rviz_ros_node = context_->getRosNodeAbstraction().lock();
   gear_sub_ =
     rviz_ros_node->get_raw_node()->create_subscription<autoware_auto_vehicle_msgs::msg::GearReport>(
-      gear_topic_property_->getTopicStd(),
-      rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
+      gear_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)),
       [this](const autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg) {
         updateGearData(msg);
       });
@@ -435,8 +386,7 @@ void SignalDisplay::topic_updated_steering()
   auto rviz_ros_node = context_->getRosNodeAbstraction().lock();
   steering_sub_ = rviz_ros_node->get_raw_node()
                     ->create_subscription<autoware_auto_vehicle_msgs::msg::SteeringReport>(
-                      steering_topic_property_->getTopicStd(),
-                      rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
+                      steering_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)),
                       [this](const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) {
                         updateSteeringData(msg);
                       });
@@ -449,8 +399,7 @@ void SignalDisplay::topic_updated_speed()
   auto rviz_ros_node = context_->getRosNodeAbstraction().lock();
   speed_sub_ = rviz_ros_node->get_raw_node()
                  ->create_subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>(
-                   speed_topic_property_->getTopicStd(),
-                   rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
+                   speed_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)),
                    [this](const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg) {
                      updateSpeedData(msg);
                    });
@@ -464,7 +413,7 @@ void SignalDisplay::topic_updated_speed_limit()
   speed_limit_sub_ =
     rviz_ros_node->get_raw_node()->create_subscription<tier4_planning_msgs::msg::VelocityLimit>(
       speed_limit_topic_property_->getTopicStd(),
-      rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
+      rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(),
       [this](const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg) {
         updateSpeedLimitData(msg);
       });
@@ -479,8 +428,7 @@ void SignalDisplay::topic_updated_turn_signals()
   turn_signals_sub_ =
     rviz_ros_node->get_raw_node()
       ->create_subscription<autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport>(
-        turn_signals_topic_property_->getTopicStd(),
-        rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
+        turn_signals_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)),
         [this](const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::SharedPtr msg) {
           updateTurnSignalsData(msg);
         });
@@ -495,8 +443,7 @@ void SignalDisplay::topic_updated_hazard_lights()
   hazard_lights_sub_ =
     rviz_ros_node->get_raw_node()
       ->create_subscription<autoware_auto_vehicle_msgs::msg::HazardLightsReport>(
-        hazard_lights_topic_property_->getTopicStd(),
-        rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
+        hazard_lights_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)),
         [this](const autoware_auto_vehicle_msgs::msg::HazardLightsReport::SharedPtr msg) {
           updateHazardLightsData(msg);
         });
@@ -509,8 +456,7 @@ void SignalDisplay::topic_updated_traffic()
   auto rviz_ros_node = context_->getRosNodeAbstraction().lock();
   traffic_sub_ = rviz_ros_node->get_raw_node()
                    ->create_subscription<autoware_perception_msgs::msg::TrafficSignal>(
-                     traffic_topic_property_->getTopicStd(),
-                     rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(),
+                     traffic_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)),
                      [this](const autoware_perception_msgs::msg::TrafficSignal::SharedPtr msg) {
                        updateTrafficLightData(msg);
                      });

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp

@@ -85,7 +85,7 @@ void SpeedDisplay::drawSpeedDisplay(QPainter & painter, const QRectF & backgroun
     backgroundRect.width() / 2 - referenceRect.width() / 2 - 5, backgroundRect.height() / 2);
 
   QString speedNumber = QString::number(current_speed_, 'f', 0);
-  int fontSize = 60;
+  int fontSize = 40;
   QFont speedFont("Quicksand", fontSize);
   painter.setFont(speedFont);
 
@@ -94,16 +94,17 @@ void SpeedDisplay::drawSpeedDisplay(QPainter & painter, const QRectF & backgroun
 
   // Center the speed number in the backgroundRect
   QPointF speedPos(
-    backgroundRect.center().x() - speedNumberRect.width() / 2, backgroundRect.center().y());
+    backgroundRect.center().x() - speedNumberRect.width() / 2,
+    backgroundRect.center().y() + speedNumberRect.bottom());
   painter.setPen(gray);
   painter.drawText(speedPos, speedNumber);
 
-  QFont unitFont("Quicksand", 14);
+  QFont unitFont("Quicksand", 8, QFont::DemiBold);
   painter.setFont(unitFont);
   QString speedUnit = "km/h";
   QRect unitRect = painter.fontMetrics().boundingRect(speedUnit);
   QPointF unitPos(
-    (backgroundRect.width() / 2 - unitRect.width() / 2), referencePos.y() + unitRect.height());
+    (backgroundRect.width() / 2 - unitRect.width() / 2), referencePos.y() + unitRect.height() + 15);
   painter.drawText(unitPos, speedUnit);
 }
 

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp

@@ -24,6 +24,7 @@
 #include <OgreTechnique.h>
 #include <OgreTexture.h>
 #include <OgreTextureManager.h>
+#include <qobject.h>
 
 #include <algorithm>
 #include <cmath>
@@ -99,10 +100,9 @@ void SpeedLimitDisplay::drawSpeedLimitIndicator(QPainter & painter, const QRectF
   }
 
   // Define the area for the outer circle
-  QRectF outerCircleRect = backgroundRect;
-  outerCircleRect.setWidth(backgroundRect.width() - 30);
-  outerCircleRect.setHeight(backgroundRect.width() - 30);
-  outerCircleRect.moveTopLeft(QPointF(backgroundRect.left() + 15, backgroundRect.top() + 10));
+  QRectF outerCircleRect = QRectF(45, 45, 45, 45);
+  outerCircleRect.moveTopRight(
+    QPointF(backgroundRect.right() - 44, backgroundRect.top() + outerCircleRect.height() / 2 + 5));
 
   // Now use borderColor for drawing
   painter.setPen(QPen(borderColor, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
@@ -112,24 +112,30 @@ void SpeedLimitDisplay::drawSpeedLimitIndicator(QPainter & painter, const QRectF
 
   // Define the area for the inner circle
   QRectF innerCircleRect = outerCircleRect;
-  innerCircleRect.setWidth(outerCircleRect.width() / 1.25);
-  innerCircleRect.setHeight(outerCircleRect.height() / 1.25);
+  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(0, 0, 0);  // Hue, Saturation, Value
-
+  colorFromHSV.setHsv(0, 0, 29);  // Hue, Saturation, Value
+  colorFromHSV.setAlphaF(0.60);   // Transparency
   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", 24, QFont::Bold);
+  QFont font = QFont("Quicksand", 16, QFont::Bold);
 
   painter.setFont(font);
   // #C2C2C2

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp

@@ -82,8 +82,8 @@ void SteeringWheelDisplay::drawSteeringWheel(QPainter & painter, const QRectF &
   // Rotate the wheel
   float steeringAngle = steering_angle_;  // No need to round here
   // Calculate the position
-  int wheelCenterX = backgroundRect.right() - wheel.width() - 17.5;
-  int wheelCenterY = backgroundRect.height() - wheel.height() + 15;
+  int wheelCenterX = backgroundRect.left() + wheel.width() + 54 + 54;
+  int wheelCenterY = backgroundRect.height() / 2;
 
   // Rotate the wheel image
   QTransform rotationTransform;

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp

@@ -62,12 +62,10 @@ void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF
   painter.setBrush(QBrush(tl_gray_, Qt::SolidPattern));
   painter.setPen(Qt::NoPen);
   // Define the area for the circle (background)
-  QRectF circleRect = backgroundRect;
-  circleRect.setWidth(backgroundRect.width() - 30);
-  circleRect.setHeight(backgroundRect.width() - 30);
+  QRectF circleRect = QRectF(50, 50, 50, 50);
   circleRect.moveTopRight(QPointF(
-    backgroundRect.left() + circleRect.width() + 15,
-    backgroundRect.top() + circleRect.height() + 30));
+    backgroundRect.right() - circleRect.width() - 75,
+    backgroundRect.top() + circleRect.height() / 2));
   painter.drawEllipse(circleRect);
 
   if (!current_traffic_.elements.empty()) {
@@ -96,7 +94,7 @@ void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF
   }
 
   // Scaling factor (e.g., 1.5 for 150% size)
-  float scaleFactor = 1.00;
+  float scaleFactor = 0.75;
 
   // Calculate the scaled size
   QSize scaledSize = traffic_light_image_.size() * scaleFactor;

common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp

@@ -72,12 +72,12 @@ void TurnSignalsDisplay::updateHazardLightsData(
 void TurnSignalsDisplay::drawArrows(
   QPainter & painter, const QRectF & backgroundRect, const QColor & color)
 {
-  QImage scaledLeftArrow = arrowImage.scaled(64, 43, Qt::KeepAspectRatio, Qt::SmoothTransformation);
+  QImage scaledLeftArrow = arrowImage.scaled(50, 32, Qt::KeepAspectRatio, Qt::SmoothTransformation);
   scaledLeftArrow = coloredImage(scaledLeftArrow, gray);
   QImage scaledRightArrow = scaledLeftArrow.mirrored(true, false);
-  int arrowYPos = (backgroundRect.height() / 3 - scaledLeftArrow.height() / 2);
-  int leftArrowXPos = backgroundRect.width() / 4 - scaledLeftArrow.width();  // Adjust as needed
-  int rightArrowXPos = backgroundRect.width() * 3 / 4;                       // Adjust as needed
+  int arrowYPos = (backgroundRect.height() / 2 - scaledLeftArrow.height() / 2 - 4);
+  int leftArrowXPos = backgroundRect.left() + scaledLeftArrow.width() * 2 + 180;
+  int rightArrowXPos = backgroundRect.right() - scaledRightArrow.width() * 3 - 175;
 
   bool leftActive =
     (current_turn_signal_ == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT ||

common/component_interface_tools/CMakeLists.txt

@@ -3,5 +3,14 @@ project(component_interface_tools)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
-ament_auto_add_executable(service_log_checker src/service_log_checker.cpp)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/service_log_checker.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "ServiceLogChecker"
+  EXECUTABLE service_log_checker_node
+)
+
 ament_auto_package(INSTALL_TO_SHARE launch)

common/component_interface_tools/launch/service_log_checker.launch.xml

@@ -1,3 +1,3 @@
 <launch>
-  <node pkg="component_interface_tools" exec="service_log_checker" name="service_log_checker"/>
+  <node pkg="component_interface_tools" exec="service_log_checker_node"/>
 </launch>

common/component_interface_tools/package.xml

@@ -13,6 +13,7 @@
   <depend>diagnostic_updater</depend>
   <depend>fmt</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
   <depend>tier4_system_msgs</depend>
   <depend>yaml_cpp_vendor</depend>
 

common/component_interface_tools/src/service_log_checker.cpp

@@ -22,7 +22,8 @@
 #define FMT_HEADER_ONLY
 #include <fmt/format.h>
 
-ServiceLogChecker::ServiceLogChecker() : Node("service_log_checker"), diagnostics_(this)
+ServiceLogChecker::ServiceLogChecker(const rclcpp::NodeOptions & options)
+: Node("service_log_checker", options), diagnostics_(this)
 {
   sub_ = create_subscription<ServiceLog>(
     "/service_log", 50, std::bind(&ServiceLogChecker::on_service_log, this, std::placeholders::_1));
@@ -98,13 +99,5 @@ void ServiceLogChecker::update_diagnostics(diagnostic_updater::DiagnosticStatusW
   }
 }
 
-int main(int argc, char ** argv)
-{
-  rclcpp::init(argc, argv);
-  rclcpp::executors::SingleThreadedExecutor executor;
-  auto node = std::make_shared<ServiceLogChecker>();
-  executor.add_node(node);
-  executor.spin();
-  executor.remove_node(node);
-  rclcpp::shutdown();
-}
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(ServiceLogChecker)

common/component_interface_tools/src/service_log_checker.hpp

@@ -26,7 +26,7 @@
 class ServiceLogChecker : public rclcpp::Node
 {
 public:
-  ServiceLogChecker();
+  explicit ServiceLogChecker(const rclcpp::NodeOptions & options);
 
 private:
   using ServiceLog = tier4_system_msgs::msg::ServiceLog;

common/glog_component/CMakeLists.txt

@@ -8,7 +8,7 @@ autoware_package()
 ament_auto_add_library(glog_component SHARED
   src/glog_component.cpp
 )
-target_link_libraries(glog_component glog)
+target_link_libraries(glog_component glog::glog)
 
 rclcpp_components_register_node(glog_component
   PLUGIN "GlogComponent"

common/glog_component/package.xml

@@ -13,7 +13,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
-  <depend>libgoogle-glog-dev</depend>
+  <depend>glog</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
 

common/glog_component/src/glog_component.cpp

@@ -17,8 +17,10 @@
 GlogComponent::GlogComponent(const rclcpp::NodeOptions & node_options)
 : Node("glog_component", node_options)
 {
-  google::InitGoogleLogging("glog_component");
-  google::InstallFailureSignalHandler();
+  if (!google::IsGoogleLoggingInitialized()) {
+    google::InitGoogleLogging("glog_component");
+    google::InstallFailureSignalHandler();
+  }
 }
 
 #include <rclcpp_components/register_node_macro.hpp>

common/motion_utils/CMakeLists.txt

@@ -7,15 +7,7 @@ autoware_package()
 find_package(Boost REQUIRED)
 
 ament_auto_add_library(motion_utils SHARED
-  src/distance/distance.cpp
-  src/marker/marker_helper.cpp
-  src/marker/virtual_wall_marker_creator.cpp
-  src/resample/resample.cpp
-  src/trajectory/trajectory.cpp
-  src/trajectory/interpolation.cpp
-  src/trajectory/path_with_lane_id.cpp
-  src/trajectory/conversion.cpp
-  src/vehicle/vehicle_state_checker.cpp
+  DIRECTORY src
 )
 
 if(BUILD_TESTING)

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp → common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp

@@ -1,5 +1,5 @@
 
-// Copyright 2022 TIER IV, Inc.
+// Copyright 2022-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.
@@ -13,8 +13,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_
-#define BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_
+#ifndef MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_
+#define MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_
 
 #include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
 #include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
@@ -25,35 +25,31 @@
 #include <string>
 #include <vector>
 
-namespace behavior_velocity_planner
+namespace motion_utils
 {
-
 using autoware_adapi_v1_msgs::msg::PlanningBehavior;
 using autoware_adapi_v1_msgs::msg::VelocityFactor;
-using autoware_adapi_v1_msgs::msg::VelocityFactorArray;
-using geometry_msgs::msg::Pose;
 using VelocityFactorBehavior = VelocityFactor::_behavior_type;
 using VelocityFactorStatus = VelocityFactor::_status_type;
+using geometry_msgs::msg::Pose;
 
 class VelocityFactorInterface
 {
 public:
-  VelocityFactorInterface() { behavior_ = VelocityFactor::UNKNOWN; }
-
-  VelocityFactor get() const { return velocity_factor_; }
-  void init(const VelocityFactorBehavior behavior) { behavior_ = behavior; }
+  [[nodiscard]] VelocityFactor get() const { return velocity_factor_; }
+  void init(const VelocityFactorBehavior & behavior) { behavior_ = behavior; }
   void reset() { velocity_factor_.behavior = PlanningBehavior::UNKNOWN; }
 
   void set(
     const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
     const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status,
-    const std::string detail = "");
+    const std::string & detail = "");
 
 private:
-  VelocityFactorBehavior behavior_;
-  VelocityFactor velocity_factor_;
+  VelocityFactorBehavior behavior_{VelocityFactor::UNKNOWN};
+  VelocityFactor velocity_factor_{};
 };
 
-}  // namespace behavior_velocity_planner
+}  // namespace motion_utils
 
-#endif  // BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_
+#endif  // MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_

common/motion_utils/package.xml

@@ -21,6 +21,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
   <depend>builtin_interfaces</depend>

planning/behavior_velocity_planner_common/src/velocity_factor_interface.cpp → common/motion_utils/src/factor/velocity_factor_interface.cpp

@@ -1,4 +1,4 @@
-// Copyright 2023 TIER IV, Inc.
+// Copyright 2023-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.
@@ -12,23 +12,24 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <behavior_velocity_planner_common/velocity_factor_interface.hpp>
+#include <motion_utils/factor/velocity_factor_interface.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
 
-namespace behavior_velocity_planner
+namespace motion_utils
 {
 void VelocityFactorInterface::set(
   const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
   const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status,
-  const std::string detail)
+  const std::string & detail)
 {
   const auto & curr_point = curr_pose.position;
   const auto & stop_point = stop_pose.position;
   velocity_factor_.behavior = behavior_;
   velocity_factor_.pose = stop_pose;
-  velocity_factor_.distance = motion_utils::calcSignedArcLength(points, curr_point, stop_point);
+  velocity_factor_.distance =
+    static_cast<float>(motion_utils::calcSignedArcLength(points, curr_point, stop_point));
   velocity_factor_.status = status;
   velocity_factor_.detail = detail;
 }
 
-}  // namespace behavior_velocity_planner
+}  // namespace motion_utils

common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.cpp

@@ -34,13 +34,19 @@ namespace tier4_calibration_rviz_plugin
 AccelBrakeMapCalibratorButtonPanel::AccelBrakeMapCalibratorButtonPanel(QWidget * parent)
 : rviz_common::Panel(parent)
 {
-  topic_label_ = new QLabel("Topic name of update suggest ");
+  topic_label_ = new QLabel("topic: ");
   topic_label_->setAlignment(Qt::AlignCenter);
 
   topic_edit_ =
     new QLineEdit("/vehicle/calibration/accel_brake_map_calibrator/output/update_suggest");
   connect(topic_edit_, SIGNAL(textEdited(QString)), SLOT(editTopic()));
 
+  service_label_ = new QLabel("service: ");
+  service_label_->setAlignment(Qt::AlignCenter);
+
+  service_edit_ = new QLineEdit("/vehicle/calibration/accel_brake_map_calibrator/update_map_dir");
+  connect(service_edit_, SIGNAL(textEdited(QString)), SLOT(editService()));
+
   calibration_button_ = new QPushButton("Wait for subscribe topic");
   calibration_button_->setEnabled(false);
   connect(calibration_button_, SIGNAL(clicked(bool)), SLOT(pushCalibrationButton()));
@@ -56,8 +62,13 @@ AccelBrakeMapCalibratorButtonPanel::AccelBrakeMapCalibratorButtonPanel(QWidget *
   topic_layout->addWidget(topic_label_);
   topic_layout->addWidget(topic_edit_);
 
+  auto * service_layout = new QHBoxLayout;
+  service_layout->addWidget(service_label_);
+  service_layout->addWidget(service_edit_);
+
   auto * v_layout = new QVBoxLayout;
   v_layout->addLayout(topic_layout);
+  v_layout->addLayout(service_layout);
   v_layout->addWidget(calibration_button_);
   v_layout->addWidget(status_label_);
 
@@ -75,7 +86,7 @@ void AccelBrakeMapCalibratorButtonPanel::onInitialize()
       &AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest, this, std::placeholders::_1));
 
   client_ = raw_node->create_client<tier4_vehicle_msgs::srv::UpdateAccelBrakeMap>(
-    "/vehicle/calibration/accel_brake_map_calibrator/update_map_dir");
+    service_edit_->text().toStdString());
 }
 
 void AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest(
@@ -85,6 +96,12 @@ void AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest(
     return;
   }
 
+  if (!client_ || !client_->service_is_ready()) {
+    calibration_button_->setText("wait for service");
+    calibration_button_->setEnabled(false);
+    return;
+  }
+
   if (msg->data) {
     status_label_->setText("Ready");
     status_label_->setStyleSheet("QLabel { background-color : white;}");
@@ -98,17 +115,34 @@ void AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest(
 
 void AccelBrakeMapCalibratorButtonPanel::editTopic()
 {
-  update_suggest_sub_.reset();
   rclcpp::Node::SharedPtr raw_node =
     this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
-  update_suggest_sub_ = raw_node->create_subscription<std_msgs::msg::Bool>(
-    topic_edit_->text().toStdString(), 10,
-    std::bind(
-      &AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest, this, std::placeholders::_1));
+  try {
+    update_suggest_sub_.reset();
+    update_suggest_sub_ = raw_node->create_subscription<std_msgs::msg::Bool>(
+      topic_edit_->text().toStdString(), 10,
+      std::bind(
+        &AccelBrakeMapCalibratorButtonPanel::callbackUpdateSuggest, this, std::placeholders::_1));
+  } catch (const rclcpp::exceptions::InvalidTopicNameError & e) {
+    RCLCPP_WARN_STREAM(raw_node->get_logger(), e.what());
+  }
   calibration_button_->setText("Wait for subscribe topic");
   calibration_button_->setEnabled(false);
 }
 
+void AccelBrakeMapCalibratorButtonPanel::editService()
+{
+  rclcpp::Node::SharedPtr raw_node =
+    this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
+  try {
+    client_.reset();
+    client_ = raw_node->create_client<tier4_vehicle_msgs::srv::UpdateAccelBrakeMap>(
+      service_edit_->text().toStdString());
+  } catch (const rclcpp::exceptions::InvalidServiceNameError & e) {
+    RCLCPP_WARN_STREAM(raw_node->get_logger(), e.what());
+  }
+}
+
 void AccelBrakeMapCalibratorButtonPanel::pushCalibrationButton()
 {
   // lock button

common/tier4_calibration_rviz_plugin/src/accel_brake_map_calibrator_button_panel.hpp

@@ -46,6 +46,7 @@ class AccelBrakeMapCalibratorButtonPanel : public rviz_common::Panel
 
 public Q_SLOTS:  // NOLINT for Qt
   void editTopic();
+  void editService();
   void pushCalibrationButton();
 
 protected:
@@ -56,6 +57,8 @@ public Q_SLOTS:  // NOLINT for Qt
 
   QLabel * topic_label_;
   QLineEdit * topic_edit_;
+  QLabel * service_label_;
+  QLineEdit * service_edit_;
   QPushButton * calibration_button_;
   QLabel * status_label_;
 };

common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml

@@ -33,6 +33,8 @@ Planning:
       logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner
     - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
       logger_name: tier4_autoware_utils
+    - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
+      logger_name: behavior_path_planner.path_shifter
 
   behavior_path_planner_avoidance:
     - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner

common/tier4_simulated_clock_rviz_plugin/README.md

@@ -10,18 +10,37 @@ This plugin allows publishing and controlling the simulated ROS time.
 | -------- | --------------------------- | -------------------------- |
 | `/clock` | `rosgraph_msgs::msg::Clock` | the current simulated time |
 
-## HowToUse
+## How to use the plugin
+
+1. Launch [planning simulator](https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/planning-simulation/#1-launch-autoware) with `use_sim_time:=true`.
+
+   ```shell
+   ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit use_sim_time:=true
+   ```
+
+   > <span style="color: orange; font-weight: bold;">Warning</span>
+   > If you launch the planning simulator without adding the `tier4_simulated_clock_rviz_plugin`, your simulation will not be running. You'll not even be able to place the initial and the goal poses.
+
+2. Start rviz and select panels/Add new panel.
 
-1. Start rviz and select panels/Add new panel.
    ![select_panel](./images/select_panels.png)
-2. Select tier4_clock_rviz_plugin/SimulatedClock and press OK.
+
+3. Select tier4_clock_rviz_plugin/SimulatedClock and press OK.
+
    ![select_clock_plugin](./images/select_clock_plugin.png)
-3. Use the added panel to control how the simulated clock is published.
+
+4. Use the added panel to control how the simulated clock is published.
+
    ![use_clock_plugin](./images/use_clock_plugin.png)
 
-   - Pause button: pause/resume the clock.
-   - Speed: speed of the clock relative to the system clock.
-   - Rate: publishing rate of the clock.
-   - Step button: advance the clock by the specified time step.
-   - Time step: value used to advance the clock when pressing the step button d).
-   - Time unit: time unit associated with the value from e).
+   <ol type="a">
+     <li>Pause button: pause/resume the clock.</li>
+     <li>Speed: speed of the clock relative to the system clock.</li>
+     <li>Rate: publishing rate of the clock.</li>
+     <li>Step button: advance the clock by the specified time step.</li>
+     <li>Time step: value used to advance the clock when pressing the step button d).</li>
+     <li>Time unit: time unit associated with the value from e).</li>
+   </ol>
+
+   > <span style="color: orange; font-weight: bold;">Warning</span>
+   > If you set the time step too large, your simulation will go haywire.

common/tvm_utility/example/yolo_v2_tiny/main.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021-2022 Arm Limited and Contributors.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -200,6 +200,8 @@
             }
           }
 
+          if (max_ind == -1) continue;
+
           // Decode and copy class probabilities
           std::vector<float> class_probabilities{};
           float p_total = 0;

control/autonomous_emergency_braking/README.md

@@ -108,6 +108,33 @@ After Noise filtering, it performs a geometric collision check to determine whet
 
 Finally, the vertex that is closest to the ego vehicle is chosen as the candidate for collision checking: Since rss distance is used to judge if a collision will happen or not, if the closest vertex to the ego is deemed to be safe, the rest of the vertices (and the points in the clusters) will also be safe.
 
+#### Obstacle velocity estimation
+
+Once the position of the closest obstacle/point is determined, the AEB modules uses the history of previously detected objects to estimate the closest object speed using the following equations:
+
+$$
+d_{t} = o_{time stamp} - prev_{time stamp}
+$$
+
+$$
+d_{pos} = norm(o_{pos} - prev_{pos})
+$$
+
+$$
+v_{norm} = d_{pos} / d_{t}
+$$
+
+Where $o_{time stamp}$ and $prev_{time stamp}$ are the timestamps of the point clouds used to detect the current closest object and the closest object of the previous point cloud frame, and $o_{pos}$ and $prev_{pos}$ are the positions of those objects, respectively.
+
+Finally, the velocity vector is compared against the ego's predicted path to get the longitudinal velocity $v_{obj}$:
+
+$$
+v_{obj} = v_{norm} * Cos(yaw_{diff}) + v_{ego}
+$$
+
+where $yaw_{diff}$ is the difference in yaw between the ego path and the displacement vector $$v_{pos} = o_{pos} - prev_{pos} $$ and $v_{ego}$ is the ego's speed, which accounts for the movement of points caused by the ego moving and not the object.
+All these equations are performed disregarding the z axis (in 2D).
+
 ### 4. Collision check with target obstacles
 
 In the fourth step, it checks the collision with filtered obstacles using RSS distance. RSS is formulated as:

control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml

@@ -1,26 +1,38 @@
 /**:
   ros__parameters:
-    publish_debug_pointcloud: false
+    # Ego path calculation
     use_predicted_trajectory: true
-    use_imu_path: true
-    path_footprint_extra_margin: 1.0
+    use_imu_path: false
+    min_generated_path_length: 0.5
+    imu_prediction_time_horizon: 1.5
+    imu_prediction_time_interval: 0.1
+    mpc_prediction_time_horizon: 1.5
+    mpc_prediction_time_interval: 0.1
+
+    # Debug
+    publish_debug_pointcloud: false
+
+    # Point cloud partitioning
     detection_range_min_height: 0.0
     detection_range_max_height_margin: 0.0
     voxel_grid_x: 0.05
     voxel_grid_y: 0.05
     voxel_grid_z: 100000.0
-    min_generated_path_length: 0.5
+
+    # Point cloud cropping
     expand_width: 0.1
+    path_footprint_extra_margin: 4.0
+
+    # Point cloud clustering
+    cluster_tolerance: 0.1 #[m]
+    minimum_cluster_size: 10
+    maximum_cluster_size: 10000
+
+    # RSS distance collision check
     longitudinal_offset: 2.0
     t_response: 1.0
     a_ego_min: -3.0
     a_obj_min: -1.0
-    cluster_tolerance: 0.1 #[m]
-    minimum_cluster_size: 10
-    maximum_cluster_size: 10000
-    imu_prediction_time_horizon: 1.5
-    imu_prediction_time_interval: 0.1
-    mpc_prediction_time_horizon: 1.5
-    mpc_prediction_time_interval: 0.1
-    collision_keeping_sec: 0.0
+    collision_keeping_sec: 2.0
+    previous_obstacle_keep_time: 1.0
     aeb_hz: 10.0

control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp

@@ -16,6 +16,7 @@
 #define AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_
 
 #include <diagnostic_updater/diagnostic_updater.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
 #include <pcl_ros/transforms.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
@@ -40,12 +41,13 @@
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
+#include <deque>
+#include <limits>
 #include <memory>
-#include <mutex>
 #include <optional>
 #include <string>
+#include <utility>
 #include <vector>
-
 namespace autoware::motion::control::autonomous_emergency_braking
 {
 
@@ -79,30 +81,146 @@ class CollisionDataKeeper
 public:
   explicit CollisionDataKeeper(rclcpp::Clock::SharedPtr clock) { clock_ = clock; }
 
-  void setTimeout(const double timeout_sec) { timeout_sec_ = timeout_sec; }
+  void setTimeout(const double collision_keep_time, const double previous_obstacle_keep_time)
+  {
+    collision_keep_time_ = collision_keep_time;
+    previous_obstacle_keep_time_ = previous_obstacle_keep_time;
+  }
+
+  std::pair<double, double> getTimeout()
+  {
+    return {collision_keep_time_, previous_obstacle_keep_time_};
+  }
 
-  bool checkExpired()
+  bool checkObjectDataExpired(std::optional<ObjectData> & data, const double timeout)
   {
-    if (data_ && (clock_->now() - data_->stamp).seconds() > timeout_sec_) {
-      data_.reset();
+    if (!data.has_value()) return true;
+    const auto now = clock_->now();
+    const auto & prev_obj = data.value();
+    const auto & data_time_stamp = prev_obj.stamp;
+    if ((now - data_time_stamp).nanoseconds() * 1e-9 > timeout) {
+      data = std::nullopt;
+      return true;
     }
-    return (data_ == nullptr);
+    return false;
+  }
+
+  bool checkCollisionExpired()
+  {
+    return this->checkObjectDataExpired(closest_object_, collision_keep_time_);
+  }
+
+  bool checkPreviousObjectDataExpired()
+  {
+    return this->checkObjectDataExpired(prev_closest_object_, previous_obstacle_keep_time_);
+  }
+
+  ObjectData get() const
+  {
+    return (closest_object_.has_value()) ? closest_object_.value() : ObjectData();
+  }
+
+  ObjectData getPreviousObjectData() const
+  {
+    return (prev_closest_object_.has_value()) ? prev_closest_object_.value() : ObjectData();
+  }
+
+  void setCollisionData(const ObjectData & data)
+  {
+    closest_object_ = std::make_optional<ObjectData>(data);
+  }
+
+  void setPreviousObjectData(const ObjectData & data)
+  {
+    prev_closest_object_ = std::make_optional<ObjectData>(data);
+  }
+
+  void resetVelocityHistory() { obstacle_velocity_history_.clear(); }
+
+  void updateVelocityHistory(
+    const double current_object_velocity, const rclcpp::Time & current_object_velocity_time_stamp)
+  {
+    // remove old msg from deque
+    const auto now = clock_->now();
+    obstacle_velocity_history_.erase(
+      std::remove_if(
+        obstacle_velocity_history_.begin(), obstacle_velocity_history_.end(),
+        [&](const auto & velocity_time_pair) {
+          const auto & vel_time = velocity_time_pair.second;
+          return ((now - vel_time).nanoseconds() * 1e-9 > previous_obstacle_keep_time_);
+        }),
+      obstacle_velocity_history_.end());
+    obstacle_velocity_history_.emplace_back(
+      std::make_pair(current_object_velocity, current_object_velocity_time_stamp));
   }
 
-  void update(const ObjectData & data) { data_.reset(new ObjectData(data)); }
+  std::optional<double> getMedianObstacleVelocity() const
+  {
+    if (obstacle_velocity_history_.empty()) return std::nullopt;
+    std::vector<double> raw_velocities;
+    for (const auto & vel_time_pair : obstacle_velocity_history_) {
+      raw_velocities.emplace_back(vel_time_pair.first);
+    }
+
+    const size_t med1 = (raw_velocities.size() % 2 == 0) ? (raw_velocities.size()) / 2 - 1
+                                                         : (raw_velocities.size()) / 2.0;
+    const size_t med2 = (raw_velocities.size()) / 2.0;
+    std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med1, raw_velocities.end());
+    const double vel1 = raw_velocities.at(med1);
+    std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med2, raw_velocities.end());
+    const double vel2 = raw_velocities.at(med2);
+    return (vel1 + vel2) / 2.0;
+  }
 
-  ObjectData get()
+  std::optional<double> calcObjectSpeedFromHistory(
+    const ObjectData & closest_object, const Path & path, const double current_ego_speed)
   {
-    if (data_) {
-      return *data_;
-    } else {
-      return ObjectData();
+    if (this->checkPreviousObjectDataExpired()) {
+      this->setPreviousObjectData(closest_object);
+      this->resetVelocityHistory();
+      return std::nullopt;
     }
+
+    const auto estimated_velocity_opt = std::invoke([&]() -> std::optional<double> {
+      const auto & prev_object = this->getPreviousObjectData();
+      const double p_dt =
+        (closest_object.stamp.nanoseconds() - prev_object.stamp.nanoseconds()) * 1e-9;
+      if (p_dt < std::numeric_limits<double>::epsilon()) return std::nullopt;
+      const auto & nearest_collision_point = closest_object.position;
+      const auto & prev_collision_point = prev_object.position;
+
+      const double p_dx = nearest_collision_point.x - prev_collision_point.x;
+      const double p_dy = nearest_collision_point.y - prev_collision_point.y;
+      const double p_dist = std::hypot(p_dx, p_dy);
+      const double p_yaw = std::atan2(p_dy, p_dx);
+      const double p_vel = p_dist / p_dt;
+
+      const auto nearest_idx = motion_utils::findNearestIndex(path, nearest_collision_point);
+      const auto & nearest_path_pose = path.at(nearest_idx);
+      const auto & traj_yaw = tf2::getYaw(nearest_path_pose.orientation);
+      const auto estimated_velocity = p_vel * std::cos(p_yaw - traj_yaw) + current_ego_speed;
+
+      // Current RSS distance calculation does not account for negative velocities
+      return (estimated_velocity > 0.0) ? estimated_velocity : 0.0;
+    });
+
+    if (!estimated_velocity_opt.has_value()) {
+      return std::nullopt;
+    }
+
+    const auto & estimated_velocity = estimated_velocity_opt.value();
+    this->setPreviousObjectData(closest_object);
+    this->updateVelocityHistory(estimated_velocity, closest_object.stamp);
+    return this->getMedianObstacleVelocity();
   }
 
 private:
-  std::unique_ptr<ObjectData> data_;
-  double timeout_sec_{0.0};
+  std::optional<ObjectData> prev_closest_object_{std::nullopt};
+  std::optional<ObjectData> closest_object_{std::nullopt};
+  double collision_keep_time_{0.0};
+  double previous_obstacle_keep_time_{0.0};
+
+  std::deque<std::pair<double, rclcpp::Time>> obstacle_velocity_history_;
   rclcpp::Clock::SharedPtr clock_;
 };
 
@@ -132,6 +250,8 @@ class AEB : public rclcpp::Node
   void onTimer();
   void onPredictedTrajectory(const Trajectory::ConstSharedPtr input_msg);
   void onAutowareState(const AutowareState::ConstSharedPtr input_msg);
+  rcl_interfaces::msg::SetParametersResult onParameter(
+    const std::vector<rclcpp::Parameter> & parameters);
 
   bool isDataReady();
 
@@ -152,14 +272,22 @@ class AEB : public rclcpp::Node
   void cropPointCloudWithEgoFootprintPath(
     const std::vector<Polygon2d> & ego_polys, pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_objects);
 
+  void createObjectDataUsingPointCloudClusters(
+    const Path & ego_path, const std::vector<Polygon2d> & ego_polys, const rclcpp::Time & stamp,
+    std::vector<ObjectData> & objects);
+  void cropPointCloudWithEgoFootprintPath(const std::vector<Polygon2d> & ego_polys);
+
   void addMarker(
     const rclcpp::Time & current_time, const Path & path, const std::vector<Polygon2d> & polygons,
-    const std::vector<ObjectData> & objects, const double color_r, const double color_g,
-    const double color_b, const double color_a, const std::string & ns,
-    MarkerArray & debug_markers);
+    const std::vector<ObjectData> & objects, const std::optional<ObjectData> & closest_object,
+    const double color_r, const double color_g, const double color_b, const double color_a,
+    const std::string & ns, MarkerArray & debug_markers);
 
   void addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers);
 
+  std::optional<double> calcObjectSpeedFromHistory(
+    const ObjectData & closest_object, const Path & path, const double current_ego_speed);
+
   PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr};
   VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr};
   Vector3::SharedPtr angular_velocity_ptr_{nullptr};
@@ -199,6 +327,8 @@ class AEB : public rclcpp::Node
   double mpc_prediction_time_horizon_;
   double mpc_prediction_time_interval_;
   CollisionDataKeeper collision_data_keeper_;
+  // Parameter callback
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
 };
 }  // namespace autoware::motion::control::autonomous_emergency_braking