diff --git a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp index 55c7a9c53d72d..ccedcceefaaf9 100644 --- a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp @@ -506,6 +506,10 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay vehicle_footprint_info_ = std::make_shared( vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, vehicle_info_->rear_overhang_m); + + property_vehicle_length_.setValue(vehicle_info_->vehicle_length_m); + property_vehicle_width_.setValue(vehicle_info_->vehicle_width_m); + property_rear_overhang_.setValue(vehicle_info_->rear_overhang_m); } else { const float length{property_vehicle_length_.getFloat()}; const float width{property_vehicle_width_.getFloat()}; diff --git a/common/tier4_system_rviz_plugin/CMakeLists.txt b/common/tier4_system_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..a65379c553131 --- /dev/null +++ b/common/tier4_system_rviz_plugin/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_system_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) + +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +set(HEADERS + src/mrm_summary_overlay_display.hpp + src/jsk_overlay_utils.hpp +) + +## Declare a C++ library +ament_auto_add_library(tier4_system_rviz_plugin SHARED + src/mrm_summary_overlay_display.cpp + src/jsk_overlay_utils.cpp + ${HEADERS} +) + +target_link_libraries(tier4_system_rviz_plugin + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + plugins +) diff --git a/common/tier4_system_rviz_plugin/README.md b/common/tier4_system_rviz_plugin/README.md new file mode 100644 index 0000000000000..098844c8b4091 --- /dev/null +++ b/common/tier4_system_rviz_plugin/README.md @@ -0,0 +1 @@ +# tier4_system_rviz_plugin diff --git a/common/tier4_system_rviz_plugin/package.xml b/common/tier4_system_rviz_plugin/package.xml new file mode 100644 index 0000000000000..adae997ea07aa --- /dev/null +++ b/common/tier4_system_rviz_plugin/package.xml @@ -0,0 +1,30 @@ + + + + tier4_system_rviz_plugin + 0.1.0 + The tier4_vehicle_rviz_plugin package + Koji Minoda + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_auto_system_msgs + diagnostic_msgs + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rviz_common + rviz_default_plugins + rviz_ogre_vendor + tier4_autoware_utils + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/tier4_system_rviz_plugin/plugins/plugin_description.xml b/common/tier4_system_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..a3ac26d1a304d --- /dev/null +++ b/common/tier4_system_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,6 @@ + + + + diff --git a/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.cpp b/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.cpp new file mode 100644 index 0000000000000..36c5d1ce82693 --- /dev/null +++ b/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.cpp @@ -0,0 +1,222 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "jsk_overlay_utils.hpp" + +#include + +namespace jsk_rviz_plugins +{ +ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer) +: pixel_buffer_(pixel_buffer) +{ + pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL); +} + +ScopedPixelBuffer::~ScopedPixelBuffer() +{ + pixel_buffer_->unlock(); +} + +Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() +{ + return pixel_buffer_; +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height) +{ + const Ogre::PixelBox & pixelBox = pixel_buffer_->getCurrentLock(); + Ogre::uint8 * pDest = static_cast(pixelBox.data); + memset(pDest, 0, width * height); + return QImage(pDest, width, height, QImage::Format_ARGB32); +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height, QColor & bg_color) +{ + QImage Hud = getQImage(width, height); + for (unsigned int i = 0; i < width; i++) { + for (unsigned int j = 0; j < height; j++) { + Hud.setPixel(i, j, bg_color.rgba()); + } + } + return Hud; +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight()); +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay, QColor & bg_color) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight(), bg_color); +} + +OverlayObject::OverlayObject(const std::string & name) : name_(name) +{ + std::string material_name = name_ + "Material"; + Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + overlay_ = mOverlayMgr->create(name_); + panel_ = static_cast( + mOverlayMgr->createOverlayElement("Panel", name_ + "Panel")); + panel_->setMetricsMode(Ogre::GMM_PIXELS); + + panel_material_ = Ogre::MaterialManager::getSingleton().create( + material_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + panel_->setMaterialName(panel_material_->getName()); + overlay_->add2D(panel_); +} + +OverlayObject::~OverlayObject() +{ + hide(); + panel_material_->unload(); + Ogre::MaterialManager::getSingleton().remove(panel_material_->getName()); + // Ogre::OverlayManager* mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + // mOverlayMgr->destroyOverlayElement(panel_); + // delete panel_; + // delete overlay_; +} + +std::string OverlayObject::getName() +{ + return name_; +} + +void OverlayObject::hide() +{ + if (overlay_->isVisible()) { + overlay_->hide(); + } +} + +void OverlayObject::show() +{ + if (!overlay_->isVisible()) { + overlay_->show(); + } +} + +bool OverlayObject::isTextureReady() +{ + return static_cast(texture_); +} + +void OverlayObject::updateTextureSize(unsigned int width, unsigned int height) +{ + const std::string texture_name = name_ + "Texture"; + if (width == 0) { + RCLCPP_WARN(rclcpp::get_logger("OverlayObject"), "width=0 is specified as texture size"); + width = 1; + } + if (height == 0) { + RCLCPP_WARN(rclcpp::get_logger("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 left, double top) +{ + panel_->setPosition(left, top); +} + +void OverlayObject::setDimensions(double width, double height) +{ + panel_->setDimensions(width, height); +} + +bool OverlayObject::isVisible() +{ + return overlay_->isVisible(); +} + +unsigned int OverlayObject::getTextureWidth() +{ + if (isTextureReady()) { + return texture_->getWidth(); + } else { + return 0; + } +} + +unsigned int OverlayObject::getTextureHeight() +{ + if (isTextureReady()) { + return texture_->getHeight(); + } else { + return 0; + } +} + +} // namespace jsk_rviz_plugins diff --git a/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.hpp b/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.hpp new file mode 100644 index 0000000000000..568111f241a28 --- /dev/null +++ b/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.hpp @@ -0,0 +1,138 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef JSK_OVERLAY_UTILS_HPP_ +#define JSK_OVERLAY_UTILS_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +// see OGRE/OgrePrerequisites.h +// #define OGRE_VERSION +// ((OGRE_VERSION_MAJOR << 16) | (OGRE_VERSION_MINOR << 8) | OGRE_VERSION_PATCH) +#if OGRE_VERSION < ((1 << 16) | (9 << 8) | 0) +#include +#include +#include +#include +#else +#include +#include +#include +#include +#include +#endif + +#include +#include +#include + +namespace jsk_rviz_plugins +{ +class OverlayObject; + +class ScopedPixelBuffer +{ +public: + explicit ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer); + virtual ~ScopedPixelBuffer(); + virtual Ogre::HardwarePixelBufferSharedPtr getPixelBuffer(); + virtual QImage getQImage(unsigned int width, unsigned int height); + virtual QImage getQImage(OverlayObject & overlay); + virtual QImage getQImage(unsigned int width, unsigned int height, QColor & bg_color); + virtual QImage getQImage(OverlayObject & overlay, QColor & bg_color); + +protected: + Ogre::HardwarePixelBufferSharedPtr pixel_buffer_; + +private: +}; + +// this is a class for put overlay object on rviz 3D panel. +// This class suppose to be instantiated in onInitialize method +// of rviz::Display class. +class OverlayObject +{ +public: + typedef std::shared_ptr Ptr; + + explicit OverlayObject(const std::string & name); + virtual ~OverlayObject(); + + virtual std::string getName(); + virtual void hide(); + virtual void show(); + virtual bool isTextureReady(); + virtual void updateTextureSize(unsigned int width, unsigned int height); + virtual ScopedPixelBuffer getBuffer(); + virtual void setPosition(double left, double top); + virtual void setDimensions(double width, double height); + virtual bool isVisible(); + virtual unsigned int getTextureWidth(); + virtual unsigned int getTextureHeight(); + +protected: + const std::string name_; + Ogre::Overlay * overlay_; + Ogre::PanelOverlayElement * panel_; + Ogre::MaterialPtr panel_material_; + Ogre::TexturePtr texture_; + +private: +}; + +// Ogre::Overlay* createOverlay(std::string name); +// Ogre::PanelOverlayElement* createOverlayPanel(Ogre::Overlay* overlay); +// Ogre::MaterialPtr createOverlayMaterial(Ogre::Overlay* overlay); +} // namespace jsk_rviz_plugins + +#endif // JSK_OVERLAY_UTILS_HPP_ diff --git a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp new file mode 100644 index 0000000000000..b80f06f645632 --- /dev/null +++ b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp @@ -0,0 +1,251 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "mrm_summary_overlay_display.hpp" + +#include +#include + +#include + +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ +void insertNewlines(std::string & str, const size_t max_line_length) +{ + size_t index = max_line_length; + + while (index < str.size()) { + str.insert(index, "\n"); + index = index + max_line_length + 1; + } +} + +std::optional generateMrmMessage(diagnostic_msgs::msg::DiagnosticStatus diag_status) +{ + if (diag_status.hardware_id == "" || diag_status.hardware_id == "system_error_monitor") { + return std::nullopt; + } else if (diag_status.level == diagnostic_msgs::msg::DiagnosticStatus::ERROR) { + std::string msg = "- ERROR: " + diag_status.name + " (" + diag_status.message + ")"; + insertNewlines(msg, 100); + return msg; + } else if (diag_status.level == diagnostic_msgs::msg::DiagnosticStatus::WARN) { + std::string msg = "- WARN: " + diag_status.name + " (" + diag_status.message + ")"; + insertNewlines(msg, 100); + return msg; + } + return std::nullopt; +} + +MrmSummaryOverlayDisplay::MrmSummaryOverlayDisplay() +{ + const Screen * screen_info = DefaultScreenOfDisplay(XOpenDisplay(NULL)); + + constexpr float hight_4k = 2160.0; + const float scale = static_cast(screen_info->height) / hight_4k; + const auto left = static_cast(std::round(1024 * scale)); + const auto top = static_cast(std::round(128 * scale)); + + property_text_color_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(25, 255, 240), "text color", this, SLOT(updateVisualization()), this); + property_left_ = new rviz_common::properties::IntProperty( + "Left", left, "Left of the plotter window", this, SLOT(updateVisualization()), this); + property_left_->setMin(0); + property_top_ = new rviz_common::properties::IntProperty( + "Top", top, "Top of the plotter window", this, SLOT(updateVisualization())); + property_top_->setMin(0); + + property_value_height_offset_ = new rviz_common::properties::IntProperty( + "Value height offset", 0, "Height offset of the plotter window", this, + SLOT(updateVisualization())); + property_font_size_ = new rviz_common::properties::IntProperty( + "Font Size", 10, "Font Size", this, SLOT(updateVisualization()), this); + property_font_size_->setMin(1); + property_max_letter_num_ = new rviz_common::properties::IntProperty( + "Max Letter Num", 100, "Max Letter Num", this, SLOT(updateVisualization()), this); + property_max_letter_num_->setMin(10); +} + +MrmSummaryOverlayDisplay::~MrmSummaryOverlayDisplay() +{ + if (initialized()) { + overlay_->hide(); + } +} + +void MrmSummaryOverlayDisplay::onInitialize() +{ + RTDClass::onInitialize(); + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "MrmSummaryOverlayDisplayObject" << count++; + overlay_.reset(new jsk_rviz_plugins::OverlayObject(ss.str())); + + overlay_->show(); + + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +void MrmSummaryOverlayDisplay::onEnable() +{ + subscribe(); + overlay_->show(); +} + +void MrmSummaryOverlayDisplay::onDisable() +{ + unsubscribe(); + reset(); + overlay_->hide(); +} + +void MrmSummaryOverlayDisplay::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + // MRM summary + std::vector mrm_comfortable_stop_summary_list = {}; + std::vector mrm_emergency_stop_summary_list = {}; + { + std::lock_guard message_lock(mutex_); + if (last_msg_ptr_) { + for (const auto & diag_status : last_msg_ptr_->status.diag_latent_fault) { + const std::optional msg = generateMrmMessage(diag_status); + if (msg != std::nullopt) { + mrm_comfortable_stop_summary_list.push_back(msg.value()); + } + } + for (const auto & diag_status : last_msg_ptr_->status.diag_single_point_fault) { + const std::optional msg = generateMrmMessage(diag_status); + if (msg != std::nullopt) { + mrm_emergency_stop_summary_list.push_back(msg.value()); + } + } + } + } + + // Display + QColor background_color; + background_color.setAlpha(0); + jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(background_color); + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + const int w = overlay_->getTextureWidth() - line_width_; + const int h = overlay_->getTextureHeight() - line_width_; + + // text + QColor text_color(property_text_color_->getColor()); + text_color.setAlpha(255); + painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); + QFont font = painter.font(); + font.setPixelSize(property_font_size_->getInt()); + font.setBold(true); + painter.setFont(font); + + std::ostringstream output_text; + output_text << std::fixed + << "Comfortable Stop MRM Summary: " << int(mrm_comfortable_stop_summary_list.size()) + << std::endl; + for (const auto & mrm_element : mrm_comfortable_stop_summary_list) { + output_text << mrm_element << std::endl; + } + output_text << "Emergency Stop MRM Summary: " << int(mrm_emergency_stop_summary_list.size()) + << std::endl; + for (const auto & mrm_element : mrm_emergency_stop_summary_list) { + output_text << mrm_element << std::endl; + } + + // same as above, but align on right side + painter.drawText( + 0, std::min(property_value_height_offset_->getInt(), h - 1), w, + std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignLeft | Qt::AlignTop, + output_text.str().c_str()); + painter.end(); + updateVisualization(); +} + +void MrmSummaryOverlayDisplay::processMessage( + const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) +{ + if (!isEnabled()) { + return; + } + + { + std::lock_guard message_lock(mutex_); + last_msg_ptr_ = msg_ptr; + } + + queueRender(); +} + +void MrmSummaryOverlayDisplay::updateVisualization() +{ + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::MrmSummaryOverlayDisplay, rviz_common::Display) diff --git a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp new file mode 100644 index 0000000000000..2ca0364e35d83 --- /dev/null +++ b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp @@ -0,0 +1,109 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MRM_SUMMARY_OVERLAY_DISPLAY_HPP_ +#define MRM_SUMMARY_OVERLAY_DISPLAY_HPP_ + +#include +#include + +#ifndef Q_MOC_RUN +#include "jsk_overlay_utils.hpp" + +#include +#include +#include +#include +#include + +#include + +#endif + +namespace rviz_plugins +{ +class MrmSummaryOverlayDisplay +: public rviz_common::RosTopicDisplay + +{ + Q_OBJECT + +public: + MrmSummaryOverlayDisplay(); + ~MrmSummaryOverlayDisplay() override; + + void onInitialize() override; + void onEnable() override; + void onDisable() override; + +private Q_SLOTS: + void updateVisualization(); + +protected: + void update(float wall_dt, float ros_dt) override; + void processMessage( + const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) override; + jsk_rviz_plugins::OverlayObject::Ptr overlay_; + rviz_common::properties::ColorProperty * property_text_color_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::IntProperty * property_value_height_offset_; + rviz_common::properties::FloatProperty * property_value_scale_; + rviz_common::properties::IntProperty * property_font_size_; + rviz_common::properties::IntProperty * property_max_letter_num_; + // QImage hud_; + +private: + static constexpr int line_width_ = 2; + static constexpr int hand_width_ = 4; + + std::mutex mutex_; + autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr last_msg_ptr_; +}; +} // namespace rviz_plugins + +#endif // MRM_SUMMARY_OVERLAY_DISPLAY_HPP_ diff --git a/control/control_validator/src/utils.cpp b/control/control_validator/src/utils.cpp index 2aade12d5f1cf..1b11d1e55c376 100644 --- a/control/control_validator/src/utils.cpp +++ b/control/control_validator/src/utils.cpp @@ -44,7 +44,8 @@ void insertPointInPredictedTrajectory( TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory_points) { - TrajectoryPoints reversed_trajectory_points(trajectory_points.size()); + TrajectoryPoints reversed_trajectory_points; + reversed_trajectory_points.reserve(trajectory_points.size()); std::reverse_copy( trajectory_points.begin(), trajectory_points.end(), std::back_inserter(reversed_trajectory_points)); @@ -106,7 +107,7 @@ Trajectory alignTrajectoryWithReferenceTrajectory( // predicted_trajectory: 　　　　p1-----p2-----p3----//------pN // trajectory: t1--------//------tN // ↓ - // predicted_trajectory: 　　　　 tNew--p3----//------pN + // predicted_trajectory: 　　　　 pNew--p3----//------pN // trajectory: t1--------//------tN auto predicted_trajectory_point_removed = removeFrontTrajectoryPoint( trajectory_points, modified_trajectory_points, predicted_trajectory_points); diff --git a/localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp b/localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp index d0279caf88dda..427bf513058d4 100644 --- a/localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp +++ b/localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp @@ -19,6 +19,7 @@ #include #include +#include #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp index 601e8dfa0117f..8900381060472 100644 --- a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp +++ b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include diff --git a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp index 3baefaa43163a..3838e93113066 100644 --- a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp +++ b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -111,7 +112,8 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim std::unordered_map> regulatory_element_signals_map; if (map_regulatory_elements_set_ == nullptr) { - RCLCPP_WARN(get_logger(), "Received traffic signal messages before a map"); + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, "Received traffic signal messages before a map"); return; } @@ -126,8 +128,9 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim auto add_signal_function = [&](const auto & signal, bool priority) { const auto id = signal.traffic_signal_id; if (!map_regulatory_elements_set_->count(id)) { - RCLCPP_WARN( - get_logger(), "Received a traffic signal not present in the current map (%lu)", id); + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, + "Received a traffic signal not present in the current map (%lu)", id); return; } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 8d9c8e147f532..964e0ff87594a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -177,7 +177,6 @@ class GoalPlannerModule : public SceneModuleInterface // for parking policy bool left_side_parking_{true}; - mutable bool allow_goal_modification_{false}; // need to be set in isExecutionRequested // pre-generate lane parking paths in a separate thread rclcpp::TimerBase::SharedPtr lane_parking_timer_; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index f37149f706436..faa629fc5cda1 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -273,10 +273,6 @@ bool GoalPlannerModule::isExecutionRequested() const const Pose & current_pose = planner_data_->self_odometry->pose.pose; const Pose & goal_pose = route_handler->getGoalPose(); - // if goal is shoulder lane, allow goal modification - allow_goal_modification_ = - route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(); - // check if goal_pose is in current_lanes. lanelet::ConstLanelet current_lane{}; const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index eb2859ff700af..f6d6edc6920a7 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1869,6 +1869,7 @@ void makeBoundLongitudinallyMonotonic( if (intersect_point) { Pose pose; pose.position = *intersect_point; + pose.position.z = bound_with_pose.at(i).position.z; const auto yaw = calcAzimuthAngle(*intersect_point, bound_with_pose.at(i + 1).position); pose.orientation = createQuaternionFromRPY(0.0, 0.0, yaw); monotonic_bound.push_back(pose); @@ -1902,18 +1903,18 @@ void makeBoundLongitudinallyMonotonic( std::vector ret; - ret.push_back(bound.front()); + for (size_t i = 0; i < bound.size(); i++) { + const auto & p_new = bound.at(i); + const auto duplicated_points_itr = std::find_if( + ret.begin(), ret.end(), + [&p_new](const auto & p) { return calcDistance2d(p, p_new) < 0.1; }); - for (size_t i = 0; i < bound.size() - 2; i++) { - try { - motion_utils::validateNonSharpAngle(bound.at(i), bound.at(i + 1), bound.at(i + 2)); - ret.push_back(bound.at(i + 1)); - } catch (const std::exception & e) { - continue; + if (duplicated_points_itr != ret.end() && std::next(duplicated_points_itr, 2) == ret.end()) { + ret.erase(duplicated_points_itr, ret.end()); } - } - ret.push_back(bound.back()); + ret.push_back(p_new); + } return ret; }; diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index db8e12708b9c5..7a972891fd053 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -37,6 +37,7 @@ using motion_utils::calcLateralOffset; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; +using motion_utils::calcSignedArcLengthPartialSum; using motion_utils::findNearestSegmentIndex; using motion_utils::insertTargetPoint; using motion_utils::resamplePath; @@ -231,9 +232,12 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto // Calculate stop point with margin const auto p_stop_line = getStopPointWithMargin(*path, path_intersects); - // TODO(murooka) add a guard of p_stop_line - const auto default_stop_pose = toStdOptional( - calcLongitudinalOffsetPose(path->points, p_stop_line->first, p_stop_line->second)); + + std::optional default_stop_pose = std::nullopt; + if (p_stop_line.has_value()) { + default_stop_pose = toStdOptional( + calcLongitudinalOffsetPose(path->points, p_stop_line->first, p_stop_line->second)); + } // Resample path sparsely for less computation cost constexpr double resample_interval = 4.0; @@ -748,15 +752,15 @@ Polygon2d CrosswalkModule::getAttentionArea( { const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto ego_polygon = createVehiclePolygon(planner_data_->vehicle_info_); + const auto backward_path_length = + calcSignedArcLength(sparse_resample_path.points, size_t(0), ego_pos); + const auto length_sum = calcSignedArcLengthPartialSum( + sparse_resample_path.points, size_t(0), sparse_resample_path.points.size()); Polygon2d attention_area; for (size_t j = 0; j < sparse_resample_path.points.size() - 1; ++j) { - const auto & p_ego_front = sparse_resample_path.points.at(j).point.pose; - const auto & p_ego_back = sparse_resample_path.points.at(j + 1).point.pose; - const auto front_length = - calcSignedArcLength(sparse_resample_path.points, ego_pos, p_ego_front.position); - const auto back_length = - calcSignedArcLength(sparse_resample_path.points, ego_pos, p_ego_back.position); + const auto front_length = length_sum.at(j) - backward_path_length; + const auto back_length = length_sum.at(j + 1) - backward_path_length; if (back_length < crosswalk_attention_range.first) { continue; @@ -1065,6 +1069,8 @@ void CrosswalkModule::setDistanceToStop( const auto & ego_pos = planner_data_->current_odometry->pose.position; const double dist_ego2stop = calcSignedArcLength(ego_path.points, ego_pos, *stop_pos); setDistance(dist_ego2stop); + } else { + setDistance(std::numeric_limits::lowest()); } } diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 742f87411b7d8..8eca49d9ffadf 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -22,6 +22,7 @@ #include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" #include "autoware_auto_geometry_msgs/msg/complex32.hpp" +#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_vehicle_msgs/msg/control_mode_command.hpp" #include "autoware_auto_vehicle_msgs/msg/control_mode_report.hpp" @@ -47,6 +48,7 @@ #include "sensor_msgs/msg/imu.hpp" #include "tier4_external_api_msgs/srv/initialize_pose.hpp" +#include #include #include @@ -62,6 +64,7 @@ namespace simple_planning_simulator using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_geometry_msgs::msg::Complex32; +using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_vehicle_msgs::msg::ControlModeReport; using autoware_auto_vehicle_msgs::msg::Engage; @@ -143,6 +146,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_vehicle_cmd_; rclcpp::Subscription::SharedPtr sub_ackermann_cmd_; rclcpp::Subscription::SharedPtr sub_manual_ackermann_cmd_; + rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::SharedPtr sub_init_pose_; rclcpp::Subscription::SharedPtr sub_init_twist_; rclcpp::Subscription::SharedPtr sub_trajectory_; @@ -160,6 +164,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rcl_interfaces::msg::SetParametersResult on_parameter( const std::vector & parameters); + lanelet::ConstLanelets road_lanelets_; + /* tf */ tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -179,6 +185,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node Trajectory::ConstSharedPtr current_trajectory_ptr_; bool simulate_motion_; //!< stop vehicle motion simulation if false ControlModeReport current_control_mode_; + bool enable_road_slope_simulation_; /* frame_id */ std::string simulated_frame_id_; //!< @brief simulated vehicle frame id @@ -214,7 +221,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node /** * @brief set input steering, velocity, and acceleration of the vehicle model */ - void set_input(const AckermannControlCommand & cmd); + void set_input(const AckermannControlCommand & cmd, const double acc_by_slope); /** * @brief set current_vehicle_state_ with received message @@ -226,6 +233,11 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node */ void on_hazard_lights_cmd(const HazardLightsCommand::ConstSharedPtr msg); + /** + * @brief subscribe lanelet map + */ + void on_map(const HADMapBin::ConstSharedPtr msg); + /** * @brief set initial pose for simulation with received message */ @@ -276,6 +288,12 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node */ TransformStamped get_transform_msg(const std::string parent_frame, const std::string child_frame); + /** + * @brief calculate ego pitch angle from trajectory + * @return ego pitch angle + */ + double calculate_ego_pitch() const; + /** * @brief timer callback for simulation with loop_rate */ diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 7837c61ec6593..2f9c2cfe333f4 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -53,6 +53,7 @@ def launch_setup(context, *args, **kwargs): }, ], remappings=[ + ("input/vector_map", "/map/vector_map"), ("input/initialpose", "/initialpose3d"), ("input/ackermann_control_command", "/control/command/control_cmd"), ("input/manual_ackermann_control_command", "/vehicle/command/manual_control_cmd"), diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index c83433d35bb1b..5652be138b18f 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -12,10 +12,14 @@ autoware_cmake autoware_auto_control_msgs + autoware_auto_mapping_msgs autoware_auto_planning_msgs autoware_auto_tf2 autoware_auto_vehicle_msgs geometry_msgs + lanelet2_core + lanelet2_extension + motion_utils nav_msgs rclcpp rclcpp_components diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index cfc58cdaeed63..04135ae40170b 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -15,11 +15,17 @@ #include "simple_planning_simulator/simple_planning_simulator_core.hpp" #include "autoware_auto_tf2/tf2_autoware_auto_msgs.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp_components/register_node_macro.hpp" #include "simple_planning_simulator/vehicle_model/sim_model.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" +#include +#include + +#include +#include #include #include @@ -45,13 +51,14 @@ autoware_auto_vehicle_msgs::msg::VelocityReport to_velocity_report( return velocity; } -nav_msgs::msg::Odometry to_odometry(const std::shared_ptr vehicle_model_ptr) +nav_msgs::msg::Odometry to_odometry( + const std::shared_ptr vehicle_model_ptr, const double ego_pitch_angle) { nav_msgs::msg::Odometry odometry; odometry.pose.pose.position.x = vehicle_model_ptr->getX(); odometry.pose.pose.position.y = vehicle_model_ptr->getY(); - odometry.pose.pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(vehicle_model_ptr->getYaw()); + odometry.pose.pose.orientation = tier4_autoware_utils::createQuaternionFromRPY( + 0.0, ego_pitch_angle, vehicle_model_ptr->getYaw()); odometry.twist.twist.linear.x = vehicle_model_ptr->getVx(); odometry.twist.twist.angular.z = vehicle_model_ptr->getWz(); @@ -66,6 +73,19 @@ autoware_auto_vehicle_msgs::msg::SteeringReport to_steering_report( return steer; } +std::vector convert_centerline_to_points( + const lanelet::Lanelet & lanelet) +{ + std::vector centerline_points; + for (const auto & point : lanelet.centerline()) { + geometry_msgs::msg::Point center_point; + center_point.x = point.basicPoint().x(); + center_point.y = point.basicPoint().y(); + center_point.z = point.basicPoint().z(); + centerline_points.push_back(center_point); + } + return centerline_points; +} } // namespace namespace simulation @@ -80,11 +100,15 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt origin_frame_id_ = declare_parameter("origin_frame_id", "odom"); add_measurement_noise_ = declare_parameter("add_measurement_noise", false); simulate_motion_ = declare_parameter("initial_engage_state"); + enable_road_slope_simulation_ = declare_parameter("enable_road_slope_simulation", false); using rclcpp::QoS; using std::placeholders::_1; using std::placeholders::_2; + sub_map_ = create_subscription( + "input/vector_map", rclcpp::QoS(10).transient_local(), + std::bind(&SimplePlanningSimulator::on_map, this, _1)); sub_init_pose_ = create_subscription( "input/initialpose", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialpose, this, _1)); sub_init_twist_ = create_subscription( @@ -251,6 +275,44 @@ rcl_interfaces::msg::SetParametersResult SimplePlanningSimulator::on_parameter( return result; } +double SimplePlanningSimulator::calculate_ego_pitch() const +{ + const double ego_x = vehicle_model_ptr_->getX(); + const double ego_y = vehicle_model_ptr_->getY(); + const double ego_yaw = vehicle_model_ptr_->getYaw(); + + geometry_msgs::msg::Pose ego_pose; + ego_pose.position.x = ego_x; + ego_pose.position.y = ego_y; + ego_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ego_yaw); + + // calculate prev/next point of lanelet centerline nearest to ego pose. + lanelet::Lanelet ego_lanelet; + if (!lanelet::utils::query::getClosestLaneletWithConstrains( + road_lanelets_, ego_pose, &ego_lanelet, 2.0, std::numeric_limits::max())) { + return 0.0; + } + const auto centerline_points = convert_centerline_to_points(ego_lanelet); + const size_t ego_seg_idx = + motion_utils::findNearestSegmentIndex(centerline_points, ego_pose.position); + + const auto & prev_point = centerline_points.at(ego_seg_idx); + const auto & next_point = centerline_points.at(ego_seg_idx + 1); + + // calculate ego yaw angle on lanelet coordinates + const double lanelet_yaw = std::atan2(next_point.y - prev_point.y, next_point.x - prev_point.x); + const double ego_yaw_against_lanelet = ego_yaw - lanelet_yaw; + + // calculate ego pitch angle considering ego yaw. + const double diff_z = next_point.z - prev_point.z; + const double diff_xy = std::hypot(next_point.x - prev_point.x, next_point.y - prev_point.y) / + std::cos(ego_yaw_against_lanelet); + const bool reverse_sign = std::cos(ego_yaw_against_lanelet) < 0.0; + const double ego_pitch_angle = + reverse_sign ? std::atan2(-diff_z, -diff_xy) : std::atan2(diff_z, diff_xy); + return ego_pitch_angle; +} + void SimplePlanningSimulator::on_timer() { if (!is_initialized_) { @@ -258,16 +320,21 @@ void SimplePlanningSimulator::on_timer() return; } + // calculate longitudinal acceleration by slope + const double ego_pitch_angle = calculate_ego_pitch(); + const double acc_by_slope = + enable_road_slope_simulation_ ? -9.81 * std::sin(ego_pitch_angle) : 0.0; + // update vehicle dynamics { const double dt = delta_time_.get_dt(get_clock()->now()); if (current_control_mode_.mode == ControlModeReport::AUTONOMOUS) { vehicle_model_ptr_->setGear(current_gear_cmd_.command); - set_input(current_ackermann_cmd_); + set_input(current_ackermann_cmd_, acc_by_slope); } else { vehicle_model_ptr_->setGear(current_manual_gear_cmd_.command); - set_input(current_manual_ackermann_cmd_); + set_input(current_manual_ackermann_cmd_, acc_by_slope); } if (simulate_motion_) { @@ -276,7 +343,7 @@ void SimplePlanningSimulator::on_timer() } // set current state - current_odometry_ = to_odometry(vehicle_model_ptr_); + current_odometry_ = to_odometry(vehicle_model_ptr_, ego_pitch_angle); current_odometry_.pose.pose.position.z = get_z_pose_from_trajectory( current_odometry_.pose.pose.position.x, current_odometry_.pose.pose.position.y); @@ -308,6 +375,19 @@ void SimplePlanningSimulator::on_timer() publish_tf(current_odometry_); } +void SimplePlanningSimulator::on_map(const HADMapBin::ConstSharedPtr msg) +{ + auto lanelet_map_ptr = std::make_shared(); + + lanelet::routing::RoutingGraphPtr routing_graph_ptr; + lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr; + lanelet::utils::conversion::fromBinMsg( + *msg, lanelet_map_ptr, &traffic_rules_ptr, &routing_graph_ptr); + + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr); + road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets); +} + void SimplePlanningSimulator::on_initialpose(const PoseWithCovarianceStamped::ConstSharedPtr msg) { // save initial pose @@ -344,7 +424,8 @@ void SimplePlanningSimulator::on_set_pose( response->status = tier4_api_utils::response_success(); } -void SimplePlanningSimulator::set_input(const AckermannControlCommand & cmd) +void SimplePlanningSimulator::set_input( + const AckermannControlCommand & cmd, const double acc_by_slope) { const auto steer = cmd.lateral.steering_tire_angle; const auto vel = cmd.longitudinal.speed; @@ -356,11 +437,11 @@ void SimplePlanningSimulator::set_input(const AckermannControlCommand & cmd) // TODO(Watanabe): The definition of the sign of acceleration in REVERSE mode is different // between .auto and proposal.iv, and will be discussed later. - float acc = accel; + float acc = accel + acc_by_slope; if (gear == GearCommand::NONE) { acc = 0.0; } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) { - acc = -accel; + acc = -accel - acc_by_slope; } if (