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/common/tier4_traffic_light_rviz_plugin/README.md b/common/tier4_traffic_light_rviz_plugin/README.md index 953412a917432..91fb5bc124cb7 100644 --- a/common/tier4_traffic_light_rviz_plugin/README.md +++ b/common/tier4_traffic_light_rviz_plugin/README.md @@ -8,9 +8,9 @@ This plugin panel publishes dummy traffic light signals. ### Output -| Name | Type | Description | -| ------------------------------------------------------- | -------------------------------------------------------- | ----------------------------- | -| `/perception/traffic_light_recognition/traffic_signals` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | Publish traffic light signals | +| Name | Type | Description | +| ------------------------------------------------------- | --------------------------------------------------- | ----------------------------- | +| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficSignalArray` | Publish traffic light signals | ## HowToUse diff --git a/common/tier4_traffic_light_rviz_plugin/package.xml b/common/tier4_traffic_light_rviz_plugin/package.xml index 21d8bae8f6cff..2b118b429f1af 100644 --- a/common/tier4_traffic_light_rviz_plugin/package.xml +++ b/common/tier4_traffic_light_rviz_plugin/package.xml @@ -11,7 +11,7 @@ autoware_cmake autoware_auto_mapping_msgs - autoware_auto_perception_msgs + autoware_perception_msgs lanelet2_extension libqt5-core libqt5-gui diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp index e4fb0095b8d0a..35c5a88a2f8a6 100644 --- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp +++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp @@ -34,6 +34,7 @@ #include #include +#undef signals namespace rviz_plugins { TrafficLightPublishPanel::TrafficLightPublishPanel(QWidget * parent) : rviz_common::Panel(parent) @@ -138,55 +139,55 @@ void TrafficLightPublishPanel::onSetTrafficLightState() const auto shape = light_shape_combo_->currentText(); const auto status = light_status_combo_->currentText(); - TrafficLight traffic_light; + TrafficSignalElement traffic_light; traffic_light.confidence = traffic_light_confidence_input_->value(); if (color == "RED") { - traffic_light.color = TrafficLight::RED; + traffic_light.color = TrafficSignalElement::RED; } else if (color == "AMBER") { - traffic_light.color = TrafficLight::AMBER; + traffic_light.color = TrafficSignalElement::AMBER; } else if (color == "GREEN") { - traffic_light.color = TrafficLight::GREEN; + traffic_light.color = TrafficSignalElement::GREEN; } else if (color == "WHITE") { - traffic_light.color = TrafficLight::WHITE; + traffic_light.color = TrafficSignalElement::WHITE; } else if (color == "UNKNOWN") { - traffic_light.color = TrafficLight::UNKNOWN; + traffic_light.color = TrafficSignalElement::UNKNOWN; } if (shape == "CIRCLE") { - traffic_light.shape = TrafficLight::CIRCLE; + traffic_light.shape = TrafficSignalElement::CIRCLE; } else if (shape == "LEFT_ARROW") { - traffic_light.shape = TrafficLight::LEFT_ARROW; + traffic_light.shape = TrafficSignalElement::LEFT_ARROW; } else if (shape == "RIGHT_ARROW") { - traffic_light.shape = TrafficLight::RIGHT_ARROW; + traffic_light.shape = TrafficSignalElement::RIGHT_ARROW; } else if (shape == "UP_ARROW") { - traffic_light.shape = TrafficLight::UP_ARROW; + traffic_light.shape = TrafficSignalElement::UP_ARROW; } else if (shape == "DOWN_ARROW") { - traffic_light.shape = TrafficLight::DOWN_ARROW; + traffic_light.shape = TrafficSignalElement::DOWN_ARROW; } else if (shape == "DOWN_LEFT_ARROW") { - traffic_light.shape = TrafficLight::DOWN_LEFT_ARROW; + traffic_light.shape = TrafficSignalElement::DOWN_LEFT_ARROW; } else if (shape == "DOWN_RIGHT_ARROW") { - traffic_light.shape = TrafficLight::DOWN_RIGHT_ARROW; + traffic_light.shape = TrafficSignalElement::DOWN_RIGHT_ARROW; } else if (shape == "UNKNOWN") { - traffic_light.shape = TrafficLight::UNKNOWN; + traffic_light.shape = TrafficSignalElement::UNKNOWN; } if (status == "SOLID_OFF") { - traffic_light.status = TrafficLight::SOLID_OFF; + traffic_light.status = TrafficSignalElement::SOLID_OFF; } else if (status == "SOLID_ON") { - traffic_light.status = TrafficLight::SOLID_ON; + traffic_light.status = TrafficSignalElement::SOLID_ON; } else if (status == "FLASHING") { - traffic_light.status = TrafficLight::FLASHING; + traffic_light.status = TrafficSignalElement::FLASHING; } else if (status == "UNKNOWN") { - traffic_light.status = TrafficLight::UNKNOWN; + traffic_light.status = TrafficSignalElement::UNKNOWN; } TrafficSignal traffic_signal; - traffic_signal.lights.push_back(traffic_light); - traffic_signal.map_primitive_id = traffic_light_id; + traffic_signal.elements.push_back(traffic_light); + traffic_signal.traffic_signal_id = traffic_light_id; for (auto & signal : extra_traffic_signals_.signals) { - if (signal.map_primitive_id == traffic_light_id) { + if (signal.traffic_signal_id == traffic_light_id) { signal = traffic_signal; return; } @@ -247,7 +248,7 @@ void TrafficLightPublishPanel::createWallTimer() void TrafficLightPublishPanel::onTimer() { if (enable_publish_) { - extra_traffic_signals_.header.stamp = rclcpp::Clock().now(); + extra_traffic_signals_.stamp = rclcpp::Clock().now(); pub_traffic_signals_->publish(extra_traffic_signals_); } @@ -260,35 +261,35 @@ void TrafficLightPublishPanel::onTimer() for (size_t i = 0; i < extra_traffic_signals_.signals.size(); ++i) { const auto & signal = extra_traffic_signals_.signals.at(i); - if (signal.lights.empty()) { + if (signal.elements.empty()) { continue; } - auto id_label = new QLabel(QString::number(signal.map_primitive_id)); + auto id_label = new QLabel(QString::number(signal.traffic_signal_id)); id_label->setAlignment(Qt::AlignCenter); auto color_label = new QLabel(); color_label->setAlignment(Qt::AlignCenter); - const auto & light = signal.lights.front(); + const auto & light = signal.elements.front(); switch (light.color) { - case TrafficLight::RED: + case TrafficSignalElement::RED: color_label->setText("RED"); color_label->setStyleSheet("background-color: #FF0000;"); break; - case TrafficLight::AMBER: + case TrafficSignalElement::AMBER: color_label->setText("AMBER"); color_label->setStyleSheet("background-color: #FFBF00;"); break; - case TrafficLight::GREEN: + case TrafficSignalElement::GREEN: color_label->setText("GREEN"); color_label->setStyleSheet("background-color: #7CFC00;"); break; - case TrafficLight::WHITE: + case TrafficSignalElement::WHITE: color_label->setText("WHITE"); color_label->setStyleSheet("background-color: #FFFFFF;"); break; - case TrafficLight::UNKNOWN: + case TrafficSignalElement::UNKNOWN: color_label->setText("UNKNOWN"); color_label->setStyleSheet("background-color: #808080;"); break; @@ -300,31 +301,28 @@ void TrafficLightPublishPanel::onTimer() shape_label->setAlignment(Qt::AlignCenter); switch (light.shape) { - case TrafficLight::CIRCLE: + case TrafficSignalElement::CIRCLE: shape_label->setText("CIRCLE"); break; - case TrafficLight::LEFT_ARROW: + case TrafficSignalElement::LEFT_ARROW: shape_label->setText("LEFT_ARROW"); break; - case TrafficLight::RIGHT_ARROW: + case TrafficSignalElement::RIGHT_ARROW: shape_label->setText("RIGHT_ARROW"); break; - case TrafficLight::UP_ARROW: + case TrafficSignalElement::UP_ARROW: shape_label->setText("UP_ARROW"); break; - case TrafficLight::DOWN_ARROW: + case TrafficSignalElement::DOWN_ARROW: shape_label->setText("DOWN_ARROW"); break; - case TrafficLight::DOWN_LEFT_ARROW: + case TrafficSignalElement::DOWN_LEFT_ARROW: shape_label->setText("DOWN_LEFT_ARROW"); break; - case TrafficLight::DOWN_RIGHT_ARROW: + case TrafficSignalElement::DOWN_RIGHT_ARROW: shape_label->setText("DOWN_RIGHT_ARROW"); break; - case TrafficLight::FLASHING: - shape_label->setText("FLASHING"); - break; - case TrafficLight::UNKNOWN: + case TrafficSignalElement::UNKNOWN: shape_label->setText("UNKNOWN"); break; default: @@ -335,16 +333,16 @@ void TrafficLightPublishPanel::onTimer() status_label->setAlignment(Qt::AlignCenter); switch (light.status) { - case TrafficLight::SOLID_OFF: + case TrafficSignalElement::SOLID_OFF: status_label->setText("SOLID_OFF"); break; - case TrafficLight::SOLID_ON: + case TrafficSignalElement::SOLID_ON: status_label->setText("SOLID_ON"); break; - case TrafficLight::FLASHING: + case TrafficSignalElement::FLASHING: status_label->setText("FLASHING"); break; - case TrafficLight::UNKNOWN: + case TrafficSignalElement::UNKNOWN: status_label->setText("UNKNOWN"); break; default: @@ -375,11 +373,9 @@ void TrafficLightPublishPanel::onVectorMap(const HADMapBin::ConstSharedPtr msg) std::string info = "Fetching traffic lights :"; std::string delim = " "; for (auto && tl_reg_elem_ptr : tl_reg_elems) { - for (auto && traffic_light : tl_reg_elem_ptr->trafficLights()) { - auto id = static_cast(traffic_light.id()); - info += (std::exchange(delim, ", ") + std::to_string(id)); - traffic_light_ids_.insert(id); - } + auto id = static_cast(tl_reg_elem_ptr->id()); + info += (std::exchange(delim, ", ") + std::to_string(id)); + traffic_light_ids_.insert(id); } RCLCPP_INFO_STREAM(raw_node_->get_logger(), info); received_vector_map_ = true; diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp index 75e6405417084..e54b6a301873b 100644 --- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp +++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp @@ -27,7 +27,7 @@ #include #include -#include +#include #endif #include @@ -36,10 +36,9 @@ namespace rviz_plugins { using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_perception_msgs::msg::TrafficLight; -using autoware_auto_perception_msgs::msg::TrafficSignal; -using autoware_auto_perception_msgs::msg::TrafficSignalArray; - +using autoware_perception_msgs::msg::TrafficSignal; +using autoware_perception_msgs::msg::TrafficSignalArray; +using autoware_perception_msgs::msg::TrafficSignalElement; class TrafficLightPublishPanel : public rviz_common::Panel { Q_OBJECT 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/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 59e8038e49188..62d4c5b7188ee 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -177,10 +177,6 @@ def launch_setup(context, *args, **kwargs): "~/input/traffic_signals", "/perception/traffic_light_recognition/traffic_signals", ), - ( - "~/input/external_traffic_signals", - "/external/traffic_light_recognition/traffic_signals", - ), ( "~/input/external_velocity_limit_mps", "/planning/scenario_planning/max_velocity_default", 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/marker_utils/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp index ac24591a0a283..d308f55799ce0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp @@ -59,8 +59,6 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns); -MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns); - MarkerArray makeOverhangToRoadShoulderMarkerArray( const behavior_path_planner::ObjectDataArray & objects, std::string && ns); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp index 808e76a140761..d337bae17ca6c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp @@ -26,12 +26,8 @@ namespace marker_utils::lane_change_markers { using behavior_path_planner::LaneChangePath; using visualization_msgs::msg::MarkerArray; -MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); MarkerArray showAllValidLaneChangePath( const std::vector & lanes, std::string && ns); -MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); -MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); -MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp index 657437615f809..ba8ddd9b19c46 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp @@ -104,6 +104,11 @@ MarkerArray createPredictedPathMarkerArray( const PredictedPath & ego_predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info, std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); +MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); + +MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); + +MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); } // namespace marker_utils #endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_ 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/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 0278b73b0acb4..3264ec1e9ddfd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -556,9 +556,6 @@ struct DebugData // shift path std::vector proposed_spline_shift; - // future pose - PathWithLaneId path_with_planned_velocity; - // avoidance require objects ObjectDataArray unavoidable_objects; @@ -568,6 +565,10 @@ struct DebugData // tmp for plot PathWithLaneId center_line; + // collision check debug map + utils::path_safety_checker::CollisionCheckDebugMap collision_check; + + // debug msg array AvoidanceDebugMsgArray avoidance_debug_msg_array; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index e2934a02b63d8..0dc07f0950716 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -70,7 +70,7 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( const PathWithLaneId & path, const Point & ego_pos, ObjectData & obj); double calcEnvelopeOverhangDistance( - const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose); + const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose); void setEndData( AvoidLine & al, const double length, const geometry_msgs::msg::Pose & end, const size_t end_idx, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index 87c93f49901f0..93eb0457d7cf4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -179,10 +179,10 @@ struct SafetyCheckParams struct CollisionCheckDebug { std::string unsafe_reason; ///< Reason indicating unsafe situation. - Pose current_pose{}; ///< Ego vehicle's current pose. Twist current_twist{}; ///< Ego vehicle's current velocity and rotation. - Twist object_twist{}; ///< Detected object's velocity and rotation. Pose expected_ego_pose{}; ///< Predicted future pose of ego vehicle. + Pose current_obj_pose{}; ///< Detected object's current pose. + Twist object_twist{}; ///< Detected object's velocity and rotation. Pose expected_obj_pose{}; ///< Predicted future pose of object. double rss_longitudinal{0.0}; ///< Longitudinal RSS measure. double inter_vehicle_distance{0.0}; ///< Distance between ego vehicle and object. @@ -190,9 +190,10 @@ struct CollisionCheckDebug double extended_polygon_lat_offset{0.0}; ///< Lateral offset for extended polygon. bool is_front{false}; ///< True if object is in front of ego vehicle. bool is_safe{false}; ///< True if situation is deemed safe. - std::vector lerped_path; ///< Interpolated ego vehicle path. - Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. - Polygon2d extended_obj_polygon{}; ///< Detected object's extended collision polygon. + std::vector ego_predicted_path; ///< ego vehicle's predicted path. + std::vector obj_predicted_path; ///< object's predicted path. + Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. + Polygon2d extended_obj_polygon{}; ///< Detected object's extended collision polygon. }; using CollisionCheckDebugPair = std::pair; using CollisionCheckDebugMap = diff --git a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp index 94073ad467326..bea1447b0d95a 100644 --- a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp @@ -416,12 +416,6 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const return msg; } -MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns) -{ - return createObjectsCubeMarkerArray( - objects, ns + "_cube", createMarkerScale(3.2, 1.7, 2.0), createMarkerColor(0.0, 0.0, 1.0, 0.8)); -} - MarkerArray makeOverhangToRoadShoulderMarkerArray( const ObjectDataArray & objects, std::string && ns) { diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp index aab2be6f3054e..7493982db78b1 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp @@ -36,40 +36,6 @@ using geometry_msgs::msg::Point; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerScale; -MarkerArray showObjectInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) -{ - Marker obj_marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.5, 0.5, 0.5), colors::aqua()); - - MarkerArray marker_array; - int32_t id{0}; - - marker_array.markers.reserve(obj_debug_vec.size()); - - int idx{0}; - - for (const auto & [uuid, info] : obj_debug_vec) { - obj_marker.id = ++id; - obj_marker.pose = info.current_pose; - - std::ostringstream ss; - - ss << "Idx: " << ++idx << "\nReason: " << info.unsafe_reason - << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal - << "\nEgo to obj: " << info.inter_vehicle_distance - << "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset - << "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset - << "\nPosition: " << (info.is_front ? "front" : "back") - << "\nSafe: " << (info.is_safe ? "Yes" : "No"); - - obj_marker.text = ss.str(); - - marker_array.markers.push_back(obj_marker); - } - return marker_array; -} - MarkerArray showAllValidLaneChangePath(const std::vector & lanes, std::string && ns) { if (lanes.empty()) { @@ -103,121 +69,6 @@ MarkerArray showAllValidLaneChangePath(const std::vector & lanes return marker_array; } -MarkerArray showLerpedPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) -{ - MarkerArray marker_array; - int32_t id{0}; - const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; - marker_array.markers.reserve(obj_debug_vec.size()); - - for (const auto & [uuid, info] : obj_debug_vec) { - Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.3, 0.3, 0.3), - colors::magenta()); - marker.points.reserve(info.lerped_path.size()); - - for (const auto & point : info.lerped_path) { - marker.points.push_back(point.position); - } - - marker_array.markers.push_back(marker); - } - return marker_array; -} - -MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) -{ - if (obj_debug_vec.empty()) { - return MarkerArray{}; - } - - constexpr float scale_val{0.2}; - int32_t id{0}; - const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); - Marker ego_marker = createDefaultMarker( - "map", now, ns, id, Marker::LINE_STRIP, createMarkerScale(scale_val, scale_val, scale_val), - colors::green()); - Marker obj_marker = ego_marker; - - auto text_marker = createDefaultMarker( - "map", now, ns + "_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(1.5, 1.5, 1.5), colors::white()); - - MarkerArray marker_array; - - const auto reserve_size = obj_debug_vec.size(); - - marker_array.markers.reserve(reserve_size * 4); - - int32_t idx = {0}; - - for (const auto & [uuid, info] : obj_debug_vec) { - const auto color = info.is_safe ? colors::green() : colors::red(); - const auto & ego_polygon = info.extended_ego_polygon.outer(); - const auto poly_z = info.current_pose.position.z; // temporally - ego_marker.id = ++id; - ego_marker.color = color; - ego_marker.points.reserve(ego_polygon.size()); - for (const auto & p : ego_polygon) { - ego_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z)); - } - marker_array.markers.push_back(ego_marker); - - std::ostringstream ss; - text_marker.id = ego_marker.id; - ss << ++idx; - text_marker.text = ss.str(); - text_marker.pose = info.expected_ego_pose; - - marker_array.markers.push_back(text_marker); - - const auto & obj_polygon = info.extended_obj_polygon.outer(); - obj_marker.id = ++id; - obj_marker.color = color; - obj_marker.points.reserve(obj_polygon.size()); - for (const auto & p : obj_polygon) { - obj_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z)); - } - marker_array.markers.push_back(obj_marker); - - text_marker.id = obj_marker.id; - text_marker.pose = info.expected_obj_pose; - marker_array.markers.push_back(text_marker); - - ego_marker.points.clear(); - obj_marker.points.clear(); - } - - return marker_array; -} - -MarkerArray showPolygonPose(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) -{ - const auto colors = colors::colors_list(); - const auto loop_size = std::min(colors.size(), obj_debug_vec.size()); - MarkerArray marker_array; - int32_t id{0}; - size_t idx{0}; - const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; - marker_array.markers.reserve(obj_debug_vec.size()); - - for (const auto & [uuid, info] : obj_debug_vec) { - const auto & color = colors.at(idx); - Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.2, 0.2, 0.2), color); - marker.points.reserve(2); - marker.points.push_back(info.expected_ego_pose.position); - marker.points.push_back(info.expected_obj_pose.position); - marker_array.markers.push_back(marker); - ++idx; - if (idx >= loop_size) { - break; - } - } - - return marker_array; -} - MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns) diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index f24e772d198c1..d6a237079448f 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/marker_utils/colors.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -37,7 +38,7 @@ using visualization_msgs::msg::Marker; CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj) { CollisionCheckDebug debug; - debug.current_pose = obj.initial_pose.pose; + debug.current_obj_pose = obj.initial_pose.pose; debug.current_twist = obj.initial_twist.twist; return {tier4_autoware_utils::toHexString(obj.uuid), debug}; } @@ -486,4 +487,162 @@ MarkerArray createPredictedPathMarkerArray( return msg; } +MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) +{ + if (obj_debug_vec.empty()) { + return MarkerArray{}; + } + + int32_t id{0}; + const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); + + constexpr float line_scale_val{0.2}; + const auto line_marker_scale = createMarkerScale(line_scale_val, line_scale_val, line_scale_val); + + auto default_line_marker = [&](const auto & color = colors::green()) { + return createDefaultMarker("map", now, ns, ++id, Marker::LINE_STRIP, line_marker_scale, color); + }; + + constexpr float text_scale_val{1.5}; + const auto text_marker_scale = createMarkerScale(text_scale_val, text_scale_val, text_scale_val); + + auto default_text_marker = [&]() { + return createDefaultMarker( + "map", now, ns + "_text", ++id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + text_marker_scale, colors::white()); + }; + + auto default_cube_marker = + [&](const auto & width, const auto & depth, const auto & color = colors::green()) { + return createDefaultMarker( + "map", now, ns + "_cube", ++id, visualization_msgs::msg::Marker::CUBE, + createMarkerScale(width, depth, 1.0), color); + }; + + MarkerArray marker_array; + marker_array.markers.reserve( + obj_debug_vec.size() * 5); // poly ego, text ego, poly obj, text obj, cube obj + + int32_t idx = {0}; + for (const auto & [uuid, info] : obj_debug_vec) { + const auto color = info.is_safe ? colors::green() : colors::red(); + const auto poly_z = info.current_obj_pose.position.z; // temporally + + const auto insert_polygon_marker = [&](const auto & polygon) { + marker_array.markers.emplace_back(); + auto & polygon_marker = marker_array.markers.back(); + polygon_marker = default_line_marker(color); + polygon_marker.points.reserve(polygon.outer().size()); + for (const auto & p : polygon.outer()) { + polygon_marker.points.push_back(createPoint(p.x(), p.y(), poly_z)); + } + }; + + insert_polygon_marker(info.extended_ego_polygon); + insert_polygon_marker(info.extended_obj_polygon); + + const auto str_idx = std::to_string(++idx); + const auto insert_text_marker = [&](const auto & pose) { + marker_array.markers.emplace_back(); + auto & text_marker = marker_array.markers.back(); + text_marker = default_text_marker(); + text_marker.text = str_idx; + text_marker.pose = pose; + }; + + insert_text_marker(info.expected_ego_pose); + insert_text_marker(info.expected_obj_pose); + + const auto insert_cube_marker = [&](const auto & pose) { + marker_array.markers.emplace_back(); + auto & cube_marker = marker_array.markers.back(); + cube_marker = default_cube_marker(1.0, 1.0, color); + cube_marker.pose = pose; + }; + insert_cube_marker(info.current_obj_pose); + } + return marker_array; +} + +MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) +{ + int32_t id{0}; + const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; + const auto arrow_marker_scale = createMarkerScale(1.0, 0.3, 0.3); + const auto default_arrow_marker = [&](const auto & color) { + return createDefaultMarker( + "map", current_time, ns, ++id, Marker::ARROW, arrow_marker_scale, color); + }; + + MarkerArray marker_array; + marker_array.markers.reserve(std::accumulate( + obj_debug_vec.cbegin(), obj_debug_vec.cend(), 0UL, + [&](const auto current_sum, const auto & obj_debug) { + const auto & [uuid, info] = obj_debug; + return current_sum + info.ego_predicted_path.size() + info.obj_predicted_path.size() + 2; + })); + + for (const auto & [uuid, info] : obj_debug_vec) { + const auto insert_marker = [&](const auto & path, const auto & color) { + for (const auto & pose : path) { + marker_array.markers.emplace_back(); + auto & marker = marker_array.markers.back(); + marker = default_arrow_marker(color); + marker.pose = pose.pose; + } + }; + + insert_marker(info.ego_predicted_path, colors::aqua()); + insert_marker(info.obj_predicted_path, colors::yellow()); + const auto insert_expected_pose_marker = [&](const auto & pose, const auto & color) { + // instead of checking for distance, inserting a new marker might be more efficient + marker_array.markers.emplace_back(); + auto & marker = marker_array.markers.back(); + marker = default_arrow_marker(color); + marker.pose = pose; + marker.pose.position.z += 0.05; + }; + + insert_expected_pose_marker(info.expected_ego_pose, colors::red()); + insert_expected_pose_marker(info.expected_obj_pose, colors::red()); + } + return marker_array; +} + +MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) +{ + int32_t id{0}; + auto default_text_marker = [&]() { + return createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, ++id, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.5, 0.5, 0.5), colors::aqua()); + }; + + MarkerArray marker_array; + + marker_array.markers.reserve(obj_debug_vec.size()); + + int idx{0}; + + for (const auto & [uuid, info] : obj_debug_vec) { + auto safety_check_info_text = default_text_marker(); + safety_check_info_text.pose = info.current_obj_pose; + + std::ostringstream ss; + + ss << "Idx: " << ++idx << "\nUnsafe reason: " << info.unsafe_reason + << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal + << "\nEgo to obj: " << info.inter_vehicle_distance + << "\nExtended polygon: " << (info.is_front ? "ego" : "object") + << "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset + << "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset + << "\nLast checked position: " << (info.is_front ? "obj in front ego" : "obj at back ego") + << "\nSafe: " << (info.is_safe ? "Yes" : "No"); + + safety_check_info_text.text = ss.str(); + marker_array.markers.push_back(safety_check_info_text); + } + return marker_array; +} + } // namespace marker_utils diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 543f02abe730f..36a33e5bee088 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -331,7 +331,7 @@ ObjectData AvoidanceModule::createObjectData( // Find the footprint point closest to the path, set to object_data.overhang_distance. object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( - object_data, object_closest_pose, object_data.overhang_pose.position); + object_data, data.reference_path, object_data.overhang_pose.position); // Check whether the the ego should avoid the object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; @@ -982,6 +982,9 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { avoid_lines.push_back(al_avoid); avoid_lines.push_back(al_return); + } else { + o.reason = "InvalidShiftLine"; + continue; } o.is_avoidable = true; @@ -1852,25 +1855,35 @@ bool AvoidanceModule::isSafePath( avoid_data_, planner_data_, parameters_, is_right_shift.value()); for (const auto & object : safety_check_target_objects) { + auto current_debug_data = marker_utils::createObjectDebug(object); + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape); const auto is_object_front = utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info); + const auto is_object_incoming = + std::abs(calcYawDeviation(getEgoPose(), object.initial_pose.pose)) > M_PI_2; + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( object, parameters_->check_all_predicted_path); - const auto & ego_predicted_path = - is_object_front ? ego_predicted_path_for_front_object : ego_predicted_path_for_rear_object; + const auto & ego_predicted_path = is_object_front && !is_object_incoming + ? ego_predicted_path_for_front_object + : ego_predicted_path_for_rear_object; for (const auto & obj_path : obj_predicted_paths) { - CollisionCheckDebug collision{}; if (!utils::path_safety_checker::checkCollision( shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params, - hysteresis_factor, collision)) { + hysteresis_factor, current_debug_data.second)) { + marker_utils::updateCollisionCheckDebugMap( + debug.collision_check, current_debug_data, false); + safe_count_ = 0; return false; + } else { + marker_utils::updateCollisionCheckDebugMap(debug.collision_check, current_debug_data, true); } } } @@ -2504,6 +2517,7 @@ void AvoidanceModule::updateData() void AvoidanceModule::processOnEntry() { initVariables(); + removeRTCStatus(); } void AvoidanceModule::processOnExit() @@ -2682,12 +2696,14 @@ void AvoidanceModule::updateDebugMarker( using marker_utils::createShiftGradMarkerArray; using marker_utils::createShiftLengthMarkerArray; using marker_utils::createShiftLineMarkerArray; + using marker_utils::showPolygon; + using marker_utils::showPredictedPath; + using marker_utils::showSafetyCheckInfo; using marker_utils::avoidance_marker::createAvoidLineMarkerArray; using marker_utils::avoidance_marker::createEgoStatusMarkerArray; using marker_utils::avoidance_marker::createOtherObjectsMarkerArray; using marker_utils::avoidance_marker::createOverhangFurthestLineStringMarkerArray; using marker_utils::avoidance_marker::createPredictedVehiclePositions; - using marker_utils::avoidance_marker::createUnsafeObjectsMarkerArray; using marker_utils::avoidance_marker::makeOverhangToRoadShoulderMarkerArray; using tier4_autoware_utils::appendMarkerArray; @@ -2711,9 +2727,11 @@ void AvoidanceModule::updateDebugMarker( add(createShiftLineMarkerArray(sl_arr, shifter.getBaseOffset(), ns, r, g, b, w)); }; + const auto addObjects = [&](const ObjectDataArray & objects, const auto & ns) { + add(createOtherObjectsMarkerArray(objects, ns)); + }; + add(createEgoStatusMarkerArray(data, getEgoPose(), "ego_status")); - add(createPredictedVehiclePositions( - debug.path_with_planned_velocity, "predicted_vehicle_positions")); const auto & path = data.reference_path; add(createPathMarkerArray(debug.center_line, "centerline", 0, 0.0, 0.5, 0.9)); @@ -2725,31 +2743,37 @@ void AvoidanceModule::updateDebugMarker( add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0)); add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1)); - add(createOtherObjectsMarkerArray( - data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD)); - add(createOtherObjectsMarkerArray( - data.other_objects, AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD)); - add(createOtherObjectsMarkerArray( - data.other_objects, AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL)); - add(createOtherObjectsMarkerArray( - data.other_objects, AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE)); - add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE)); - add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT)); - add(createOtherObjectsMarkerArray(data.other_objects, std::string("MovingObject"))); - add(createOtherObjectsMarkerArray(data.other_objects, std::string("CrosswalkUser"))); - add(createOtherObjectsMarkerArray(data.other_objects, std::string("OutOfTargetArea"))); - add(createOtherObjectsMarkerArray(data.other_objects, std::string("NotNeedAvoidance"))); - add(createOtherObjectsMarkerArray(data.other_objects, std::string("LessThanExecutionThreshold"))); - add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang")); add(createOverhangFurthestLineStringMarkerArray(debug.bounds, "bounds", 1.0, 0.0, 1.0)); - add(createUnsafeObjectsMarkerArray(debug.unsafe_objects, "unsafe_objects")); + // ignore objects + { + addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD); + addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD); + addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE); + addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL); + addObjects(data.other_objects, AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE); + addObjects(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT); + addObjects(data.other_objects, std::string("MovingObject")); + addObjects(data.other_objects, std::string("CrosswalkUser")); + addObjects(data.other_objects, std::string("OutOfTargetArea")); + addObjects(data.other_objects, std::string("NotNeedAvoidance")); + addObjects(data.other_objects, std::string("LessThanExecutionThreshold")); + } // parent object info - addAvoidLine(debug.registered_raw_shift, "p_registered_shift", 0.8, 0.8, 0.0); - addAvoidLine(debug.current_raw_shift, "p_current_raw_shift", 0.5, 0.2, 0.2); - addAvoidLine(debug.extra_return_shift, "p_extra_return_shift", 0.0, 0.5, 0.8); + { + addAvoidLine(debug.registered_raw_shift, "p_registered_shift", 0.8, 0.8, 0.0); + addAvoidLine(debug.current_raw_shift, "p_current_raw_shift", 0.5, 0.2, 0.2); + addAvoidLine(debug.extra_return_shift, "p_extra_return_shift", 0.0, 0.5, 0.8); + } + + // safety check + { + add(showSafetyCheckInfo(debug.collision_check, "object_debug_info")); + add(showPredictedPath(debug.collision_check, "ego_predicted_path")); + add(showPolygon(debug.collision_check, "ego_and_target_polygon_relation")); + } // shift length { 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 d30e63bc6357e..dda350d073e80 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/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp index 841fc9092e279..a8ff0c0bbcf02 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -216,7 +216,7 @@ ObjectData AvoidanceByLaneChange::createObjectData( // Find the footprint point closest to the path, set to object_data.overhang_distance. object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( - object_data, object_closest_pose, object_data.overhang_pose.position); + object_data, data.reference_path, object_data.overhang_pose.position); // Check whether the the ego should avoid the object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 072c1c62ae3d2..0ceab21573316 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -295,11 +295,10 @@ void LaneChangeInterface::setObjectDebugVisualization() const if (!parameters_->publish_debug_marker) { return; } + using marker_utils::showPolygon; + using marker_utils::showPredictedPath; + using marker_utils::showSafetyCheckInfo; using marker_utils::lane_change_markers::showAllValidLaneChangePath; - using marker_utils::lane_change_markers::showLerpedPose; - using marker_utils::lane_change_markers::showObjectInfo; - using marker_utils::lane_change_markers::showPolygon; - using marker_utils::lane_change_markers::showPolygonPose; const auto debug_data = module_type_->getDebugData(); const auto debug_after_approval = module_type_->getAfterApprovalDebugData(); @@ -312,14 +311,14 @@ void LaneChangeInterface::setObjectDebugVisualization() const add(showAllValidLaneChangePath(debug_valid_path, "lane_change_valid_paths")); if (!debug_data.empty()) { - add(showObjectInfo(debug_data, "object_debug_info")); - add(showLerpedPose(debug_data, "ego_predicted_path")); + add(showSafetyCheckInfo(debug_data, "object_debug_info")); + add(showPredictedPath(debug_data, "ego_predicted_path")); add(showPolygon(debug_data, "ego_and_target_polygon_relation")); } if (!debug_after_approval.empty()) { - add(showObjectInfo(debug_after_approval, "object_debug_info_after_approval")); - add(showLerpedPose(debug_after_approval, "ego_predicted_path_after_approval")); + add(showSafetyCheckInfo(debug_after_approval, "object_debug_info_after_approval")); + add(showPredictedPath(debug_after_approval, "ego_predicted_path_after_approval")); add(showPolygon(debug_after_approval, "ego_and_target_polygon_relation_after_approval")); } } diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 856c3220165e7..945344259fdb2 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -406,13 +406,14 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( } double calcEnvelopeOverhangDistance( - const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose) + const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose) { double largest_overhang = isOnRight(object_data) ? -100.0 : 100.0; // large number for (const auto & p : object_data.envelope_poly.outer()) { const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); - const auto lateral = tier4_autoware_utils::calcLateralDeviation(base_pose, point); + const auto idx = findFirstNearestIndex(path.points, point); + const auto lateral = calcLateralDeviation(getPose(path.points.at(idx)), point); const auto & overhang_pose_on_right = [&overhang_pose, &largest_overhang, &point, &lateral]() { if (lateral > largest_overhang) { @@ -1014,7 +1015,7 @@ void filterTargetObjects( const auto lines = rh->getFurthestLinestring(target_lanelet, get_right, get_left, get_opposite, true); const auto & line = isOnRight(o) ? lines.back() : lines.front(); - const auto d = distance2d(to2D(overhang_basic_pose), to2D(line.basicLineString())); + const auto d = boost::geometry::distance(o.envelope_poly, to2D(line.basicLineString())); if (d < o.to_road_shoulder_distance) { o.to_road_shoulder_distance = d; target_line = line; diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 2939ca29c0a40..1838dc6bdc07b 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -253,7 +253,11 @@ bool checkCollision( const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, double hysteresis_factor, CollisionCheckDebug & debug) { - debug.lerped_path.reserve(target_object_path.path.size()); + { + debug.ego_predicted_path = predicted_ego_path; + debug.obj_predicted_path = target_object_path.path; + debug.current_obj_pose = target_object.initial_pose.pose; + } for (const auto & obj_pose_with_poly : target_object_path.path) { const auto & current_time = obj_pose_with_poly.time; @@ -277,7 +281,6 @@ bool checkCollision( const auto & ego_velocity = interpolated_data->velocity; { - debug.lerped_path.push_back(ego_pose); debug.expected_ego_pose = ego_pose; debug.expected_obj_pose = obj_pose; debug.extended_ego_polygon = ego_polygon; 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/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index 75f96c9194aef..66f326ed799af 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -21,6 +21,7 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_tf2 + autoware_perception_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 7adde028f59c0..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; @@ -934,28 +938,25 @@ bool CrosswalkModule::isRedSignalForPedestrians() const crosswalk_.regulatoryElementsAs(); for (const auto & traffic_lights_reg_elem : traffic_lights_reg_elems) { - lanelet::ConstLineStringsOrPolygons3d traffic_lights = traffic_lights_reg_elem->trafficLights(); - for (const auto & traffic_light : traffic_lights) { - const auto ll_traffic_light = static_cast(traffic_light); - const auto traffic_signal_stamped = planner_data_->getTrafficSignal(ll_traffic_light.id()); - if (!traffic_signal_stamped) { - continue; - } + const auto traffic_signal_stamped = + planner_data_->getTrafficSignal(traffic_lights_reg_elem->id()); + if (!traffic_signal_stamped) { + continue; + } - if ( - planner_param_.traffic_light_state_timeout < - (clock_->now() - traffic_signal_stamped->header.stamp).seconds()) { - continue; - } + if ( + planner_param_.traffic_light_state_timeout < + (clock_->now() - traffic_signal_stamped->stamp).seconds()) { + continue; + } - const auto & lights = traffic_signal_stamped->signal.lights; - if (lights.empty()) { - continue; - } + const auto & lights = traffic_signal_stamped->signal.elements; + if (lights.empty()) { + continue; + } - if (lights.front().color == TrafficLight::RED) { - return true; - } + if (lights.front().color == TrafficSignalElement::RED) { + return true; } } @@ -1068,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/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index e0851366d7f71..65f3772cd4d33 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -50,8 +50,8 @@ namespace behavior_velocity_planner using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::TrafficLight; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::TrafficSignalElement; using lanelet::autoware::Crosswalk; using tier4_api_msgs::msg::CrosswalkStatus; using tier4_autoware_utils::Polygon2d; diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index 1d6067d4be322..eee6ca6ffb468 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -19,6 +19,7 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs + autoware_perception_msgs behavior_velocity_planner_common geometry_msgs interpolation diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index a987ce9af9dfe..6dd7b7022a1d8 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -675,9 +675,10 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane) } bool isTrafficLightArrowActivated( - lanelet::ConstLanelet lane, - const std::map & tl_infos) + lanelet::ConstLanelet lane, const std::map & tl_infos) { + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + const auto & turn_direction = lane.attributeOr("turn_direction", "else"); std::optional tl_id = std::nullopt; for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { @@ -694,16 +695,13 @@ bool isTrafficLightArrowActivated( return false; } const auto & tl_info = tl_info_it->second; - for (auto && tl_light : tl_info.signal.lights) { - if (tl_light.color != autoware_auto_perception_msgs::msg::TrafficLight::GREEN) continue; - if (tl_light.status != autoware_auto_perception_msgs::msg::TrafficLight::SOLID_ON) continue; - if ( - turn_direction == std::string("left") && - tl_light.shape == autoware_auto_perception_msgs::msg::TrafficLight::LEFT_ARROW) + for (auto && tl_light : tl_info.signal.elements) { + if (tl_light.color != TrafficSignalElement::GREEN) continue; + if (tl_light.status != TrafficSignalElement::SOLID_ON) continue; + if (turn_direction == std::string("left") && tl_light.shape == TrafficSignalElement::LEFT_ARROW) return true; if ( - turn_direction == std::string("right") && - tl_light.shape == autoware_auto_perception_msgs::msg::TrafficLight::RIGHT_ARROW) + turn_direction == std::string("right") && tl_light.shape == TrafficSignalElement::RIGHT_ARROW) return true; } return false; diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 287a75979927b..c3dd99a5a08ad 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -111,8 +111,7 @@ std::optional getIntersectionArea( bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); bool isTrafficLightArrowActivated( - lanelet::ConstLanelet lane, - const std::map & tl_infos); + lanelet::ConstLanelet lane, const std::map & tl_infos); std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets, diff --git a/planning/behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/README.md index 25546c95074c3..519e68764b450 100644 --- a/planning/behavior_velocity_planner/README.md +++ b/planning/behavior_velocity_planner/README.md @@ -29,15 +29,15 @@ So for example, in order to stop at a stop line with the vehicles' front on the ## Input topics -| Name | Type | Description | -| ----------------------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------------------------------------------------------------------- | -| `~input/path_with_lane_id` | autoware_auto_planning_msgs::msg::PathWithLaneId | path with lane_id | -| `~input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map | -| `~input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle velocity | -| `~input/dynamic_objects` | autoware_auto_perception_msgs::msg::PredictedObjects | dynamic objects | -| `~input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | -| `~/input/compare_map_filtered_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud filtered by compare map. Note that this is used only when the detection method of run out module is Points. | -| `~input/traffic_signals` | autoware_auto_perception_msgs::msg::TrafficSignalArray | traffic light states | +| Name | Type | Description | +| ----------------------------------------- | ---------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------- | +| `~input/path_with_lane_id` | autoware_auto_planning_msgs::msg::PathWithLaneId | path with lane_id | +| `~input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map | +| `~input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle velocity | +| `~input/dynamic_objects` | autoware_auto_perception_msgs::msg::PredictedObjects | dynamic objects | +| `~input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | +| `~/input/compare_map_filtered_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud filtered by compare map. Note that this is used only when the detection method of run out module is Points. | +| `~input/traffic_signals` | autoware_perception_msgs::msg::TrafficSignalArray | traffic light states | ## Output topics diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 12503f743b0f2..0aed3015fbf40 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -99,7 +99,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio std::bind(&BehaviorVelocityPlannerNode::onLaneletMap, this, _1), createSubscriptionOptions(this)); sub_traffic_signals_ = - this->create_subscription( + this->create_subscription( "~/input/traffic_signals", 1, std::bind(&BehaviorVelocityPlannerNode::onTrafficSignals, this, _1), createSubscriptionOptions(this)); @@ -287,15 +287,15 @@ void BehaviorVelocityPlannerNode::onLaneletMap( } void BehaviorVelocityPlannerNode::onTrafficSignals( - const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) { std::lock_guard lock(mutex_); for (const auto & signal : msg->signals) { - autoware_auto_perception_msgs::msg::TrafficSignalStamped traffic_signal; - traffic_signal.header = msg->header; + TrafficSignalStamped traffic_signal; + traffic_signal.stamp = msg->stamp; traffic_signal.signal = signal; - planner_data_.traffic_light_id_map[signal.map_primitive_id] = traffic_signal; + planner_data_.traffic_light_id_map[signal.traffic_signal_id] = traffic_signal; } } diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index 91172fdd77da8..98cdaeb3e0cdb 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -61,7 +61,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_vehicle_odometry_; rclcpp::Subscription::SharedPtr sub_acceleration_; rclcpp::Subscription::SharedPtr sub_lanelet_map_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_traffic_signals_; rclcpp::Subscription::SharedPtr sub_virtual_traffic_light_states_; @@ -77,7 +77,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node void onAcceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); void onLaneletMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); void onTrafficSignals( - const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); + const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); void onVirtualTrafficLightStates( const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); void onOccupancyGrid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index 44d3184a5f6b7..2668d83b0f510 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -17,14 +17,14 @@ #include "route_handler/route_handler.hpp" +#include #include #include #include #include #include -#include -#include +#include #include #include #include @@ -83,7 +83,7 @@ struct PlannerData double ego_nearest_yaw_threshold; // other internal data - std::map traffic_light_id_map; + std::map traffic_light_id_map; boost::optional external_velocity_limit; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; @@ -132,14 +132,12 @@ struct PlannerData return true; } - std::shared_ptr getTrafficSignal( - const int id) const + std::shared_ptr getTrafficSignal(const int id) const { if (traffic_light_id_map.count(id) == 0) { return {}; } - return std::make_shared( - traffic_light_id_map.at(id)); + return std::make_shared(traffic_light_id_map.at(id)); } }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp index 14b009ceb0748..14c80b59acf5e 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -78,6 +79,12 @@ struct PointWithSearchRangeIndex SearchRangeIndex index; }; +struct TrafficSignalStamped +{ + builtin_interfaces::msg::Time stamp; + autoware_perception_msgs::msg::TrafficSignal signal; +}; + using geometry_msgs::msg::Pose; using BasicPolygons2d = std::vector; using Polygons2d = std::vector; diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml index 6a7a6f698bfbc..aedbab65068fb 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -19,9 +19,9 @@ autoware_adapi_v1_msgs autoware_auto_mapping_msgs - autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_tf2 + autoware_perception_msgs diagnostic_msgs eigen geometry_msgs diff --git a/planning/behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml index a8733c5dcf5be..c658f0890b986 100644 --- a/planning/behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_traffic_light_module/package.xml @@ -18,8 +18,8 @@ eigen3_cmake_module autoware_adapi_v1_msgs - autoware_auto_perception_msgs autoware_auto_planning_msgs + autoware_perception_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index 1b0646f4bee00..f035544b81592 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -41,7 +41,7 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) planner_param_.enable_pass_judge = getOrDeclareParameter(node, ns + ".enable_pass_judge"); planner_param_.yellow_lamp_period = getOrDeclareParameter(node, ns + ".yellow_lamp_period"); - pub_tl_state_ = node.create_publisher( + pub_tl_state_ = node.create_publisher( "~/output/traffic_signal", 1); } @@ -51,9 +51,7 @@ void TrafficLightModuleManager::modifyPathVelocity( visualization_msgs::msg::MarkerArray debug_marker_array; visualization_msgs::msg::MarkerArray virtual_wall_marker_array; - autoware_auto_perception_msgs::msg::LookingTrafficSignal tl_state; - tl_state.header.stamp = path->header.stamp; - tl_state.is_module_running = false; + autoware_perception_msgs::msg::TrafficSignal tl_state; autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; velocity_factor_array.header.frame_id = "map"; diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_traffic_light_module/src/manager.hpp index 450cba09e1e08..c36c6af1128ce 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.hpp @@ -55,8 +55,7 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC const lanelet::TrafficLightConstPtr registered_element) const; // Debug - rclcpp::Publisher::SharedPtr - pub_tl_state_; + rclcpp::Publisher::SharedPtr pub_tl_state_; boost::optional first_ref_stop_path_point_index_; }; diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 679a2f0cb9320..e0633926e35df 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -159,32 +159,6 @@ bool calcStopPointAndInsertIndex( } return false; } - -geometry_msgs::msg::Point getTrafficLightPosition( - const lanelet::ConstLineStringOrPolygon3d & traffic_light) -{ - if (!traffic_light.lineString()) { - throw std::invalid_argument{"Traffic light is not LineString"}; - } - geometry_msgs::msg::Point tl_center; - auto traffic_light_ls = traffic_light.lineString().value(); - for (const auto & tl_point : traffic_light_ls) { - tl_center.x += tl_point.x() / traffic_light_ls.size(); - tl_center.y += tl_point.y() / traffic_light_ls.size(); - tl_center.z += tl_point.z() / traffic_light_ls.size(); - } - return tl_center; -} -autoware_auto_perception_msgs::msg::LookingTrafficSignal initializeTrafficSignal( - const rclcpp::Time stamp) -{ - autoware_auto_perception_msgs::msg::LookingTrafficSignal state; - state.header.stamp = stamp; - state.is_module_running = true; - state.perception.has_state = false; - state.result.has_state = false; - return state; -} } // namespace TrafficLightModule::TrafficLightModule( @@ -197,7 +171,6 @@ TrafficLightModule::TrafficLightModule( traffic_light_reg_elem_(traffic_light_reg_elem), lane_(lane), state_(State::APPROACH), - input_(Input::PERCEPTION), is_prev_state_stop_(false) { velocity_factor_.init(VelocityFactor::TRAFFIC_SIGNAL); @@ -206,7 +179,6 @@ TrafficLightModule::TrafficLightModule( bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { - looking_tl_state_ = initializeTrafficSignal(path->header.stamp); debug_data_ = DebugData(); debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; first_stop_path_point_index_ = static_cast(path->points.size()) - 1; @@ -217,9 +189,8 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const auto & self_pose = planner_data_->current_odometry; - // Get lanelet2 traffic lights and stop lines. + // Get lanelet2 stop lines. lanelet::ConstLineString3d lanelet_stop_lines = *(traffic_light_reg_elem_.stopLine()); - lanelet::ConstLineStringsOrPolygons3d traffic_lights = traffic_light_reg_elem_.trafficLights(); // Calculate stop pose and insert index Eigen::Vector2d stop_line_point{}; @@ -255,7 +226,7 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * first_ref_stop_path_point_index_ = stop_line_point_idx; // Check if stop is coming. - setSafe(!isStopSignal(traffic_lights)); + setSafe(!isStopSignal()); if (isActivated()) { is_prev_state_stop_ = false; return true; @@ -283,33 +254,27 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return false; } -bool TrafficLightModule::isStopSignal(const lanelet::ConstLineStringsOrPolygons3d & traffic_lights) +bool TrafficLightModule::isStopSignal() { - if (!updateTrafficSignal(traffic_lights)) { + if (!updateTrafficSignal()) { return false; } - return looking_tl_state_.result.judge == - autoware_auto_perception_msgs::msg::TrafficSignalWithJudge::STOP; + return isTrafficSignalStop(looking_tl_state_); } -bool TrafficLightModule::updateTrafficSignal( - const lanelet::ConstLineStringsOrPolygons3d & traffic_lights) +bool TrafficLightModule::updateTrafficSignal() { - autoware_auto_perception_msgs::msg::TrafficSignalStamped tl_state_perception; - bool found_perception = getHighestConfidenceTrafficSignal(traffic_lights, tl_state_perception); + TrafficSignal signal; + bool found_signal = findValidTrafficSignal(signal); - if (!found_perception) { + if (!found_signal) { // Don't stop when UNKNOWN or TIMEOUT as discussed at #508 - input_ = Input::NONE; return false; } - if (found_perception) { - looking_tl_state_.perception = generateTlStateWithJudgeFromTlState(tl_state_perception.signal); - looking_tl_state_.result = looking_tl_state_.perception; - input_ = Input::PERCEPTION; - } + // Found signal associated with the lanelet + looking_tl_state_ = signal; return true; } @@ -337,7 +302,7 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const const auto & enable_pass_judge = planner_param_.enable_pass_judge; - if (enable_pass_judge && !stoppable && !is_prev_state_stop_ && input_ == Input::PERCEPTION) { + if (enable_pass_judge && !stoppable && !is_prev_state_stop_) { // Cannot stop under acceleration and jerk limits. // However, ego vehicle can't enter the intersection while the light is yellow. // It is called dilemma zone. Make a stop decision to be safe. @@ -359,10 +324,9 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const } bool TrafficLightModule::isTrafficSignalStop( - const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state) const + const autoware_perception_msgs::msg::TrafficSignal & tl_state) const { - if (hasTrafficLightCircleColor( - tl_state, autoware_auto_perception_msgs::msg::TrafficLight::GREEN)) { + if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::GREEN)) { return false; } @@ -373,86 +337,46 @@ bool TrafficLightModule::isTrafficSignalStop( } if ( turn_direction == "right" && - hasTrafficLightShape(tl_state, autoware_auto_perception_msgs::msg::TrafficLight::RIGHT_ARROW)) { + hasTrafficLightShape(tl_state, TrafficSignalElement::RIGHT_ARROW)) { return false; } if ( - turn_direction == "left" && - hasTrafficLightShape(tl_state, autoware_auto_perception_msgs::msg::TrafficLight::LEFT_ARROW)) { + turn_direction == "left" && hasTrafficLightShape(tl_state, TrafficSignalElement::LEFT_ARROW)) { return false; } if ( turn_direction == "straight" && - hasTrafficLightShape(tl_state, autoware_auto_perception_msgs::msg::TrafficLight::UP_ARROW)) { + hasTrafficLightShape(tl_state, TrafficSignalElement::UP_ARROW)) { return false; } return true; } -bool TrafficLightModule::getHighestConfidenceTrafficSignal( - const lanelet::ConstLineStringsOrPolygons3d & traffic_lights, - autoware_auto_perception_msgs::msg::TrafficSignalStamped & highest_confidence_tl_state) +bool TrafficLightModule::findValidTrafficSignal(TrafficSignal & valid_traffic_signal) { - // search traffic light state - bool found = false; - double highest_confidence = 0.0; - std::string reason; - for (const auto & traffic_light : traffic_lights) { - // traffic light must be linestrings - if (!traffic_light.isLineString()) { - reason = "NotLineString"; - continue; - } - - const int id = static_cast(traffic_light).id(); - RCLCPP_DEBUG(logger_, "traffic light id: %d (on route)", id); - const auto tl_state_stamped = planner_data_->getTrafficSignal(id); - if (!tl_state_stamped) { - reason = "TrafficSignalNotFound"; - continue; - } - - const auto header = tl_state_stamped->header; - const auto tl_state = tl_state_stamped->signal; - if (!((clock_->now() - header.stamp).seconds() < planner_param_.tl_state_timeout)) { - reason = "TimeOut"; - continue; - } - - if ( - tl_state.lights.empty() || - tl_state.lights.front().color == autoware_auto_perception_msgs::msg::TrafficLight::UNKNOWN) { - reason = "TrafficLightUnknown"; - continue; - } - - if (highest_confidence < tl_state.lights.front().confidence) { - highest_confidence = tl_state.lights.front().confidence; - highest_confidence_tl_state = *tl_state_stamped; - try { - auto tl_position = getTrafficLightPosition(traffic_light); - debug_data_.traffic_light_points.push_back(tl_position); - debug_data_.highest_confidence_traffic_light_point = std::make_optional(tl_position); - } catch (const std::invalid_argument & ex) { - RCLCPP_WARN_STREAM(logger_, ex.what()); - continue; - } - found = true; - } + // get traffic signal associated with the regulatory element id + const auto traffic_signal_stamped = planner_data_->getTrafficSignal(traffic_light_reg_elem_.id()); + if (!traffic_signal_stamped) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "the traffic signal data associated with regulatory element id is not received"); + return false; } - if (!found) { - std::string not_found_traffic_ids{""}; - for (size_t i = 0; i < traffic_lights.size(); ++i) { - const int id = static_cast(traffic_lights.at(i)).id(); - not_found_traffic_ids += (i != 0 ? "," : "") + std::to_string(id); - } + // check if the traffic signal data is outdated + const auto is_traffic_signal_timeout = + (clock_->now() - traffic_signal_stamped->stamp).seconds() > planner_param_.tl_state_timeout; + if (is_traffic_signal_timeout) { RCLCPP_WARN_THROTTLE( - logger_, *clock_, 5000 /* ms */, "cannot find traffic light lamp state (%s) due to %s.", - not_found_traffic_ids.c_str(), reason.c_str()); + logger_, *clock_, 5000 /* ms */, "the received traffic signal data is outdated"); + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "time diff: " << (clock_->now() - traffic_signal_stamped->stamp).seconds()); return false; } + + valid_traffic_signal = traffic_signal_stamped->signal; return true; } @@ -496,40 +420,24 @@ autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopP } bool TrafficLightModule::hasTrafficLightCircleColor( - const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state, - const uint8_t & lamp_color) const + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color) const { - using autoware_auto_perception_msgs::msg::TrafficLight; - const auto it_lamp = - std::find_if(tl_state.lights.begin(), tl_state.lights.end(), [&lamp_color](const auto & x) { - return x.shape == TrafficLight::CIRCLE && x.color == lamp_color; + std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { + return x.shape == TrafficSignalElement::CIRCLE && x.color == lamp_color; }); - return it_lamp != tl_state.lights.end(); + return it_lamp != tl_state.elements.end(); } bool TrafficLightModule::hasTrafficLightShape( - const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state, - const uint8_t & lamp_shape) const + const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape) const { const auto it_lamp = std::find_if( - tl_state.lights.begin(), tl_state.lights.end(), + tl_state.elements.begin(), tl_state.elements.end(), [&lamp_shape](const auto & x) { return x.shape == lamp_shape; }); - return it_lamp != tl_state.lights.end(); + return it_lamp != tl_state.elements.end(); } -autoware_auto_perception_msgs::msg::TrafficSignalWithJudge -TrafficLightModule::generateTlStateWithJudgeFromTlState( - const autoware_auto_perception_msgs::msg::TrafficSignal tl_state) const -{ - autoware_auto_perception_msgs::msg::TrafficSignalWithJudge tl_state_with_judge; - tl_state_with_judge.signal = tl_state; - tl_state_with_judge.has_state = true; - tl_state_with_judge.judge = isTrafficSignalStop(tl_state) - ? autoware_auto_perception_msgs::msg::TrafficSignalWithJudge::STOP - : autoware_auto_perception_msgs::msg::TrafficSignalWithJudge::GO; - return tl_state_with_judge; -} } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index 99ece6cbca09d..a24db2c440e91 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -29,8 +29,6 @@ #include #include -#include - #include #include @@ -39,15 +37,15 @@ namespace behavior_velocity_planner class TrafficLightModule : public SceneModuleInterface { public: + using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal; + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; enum class State { APPROACH, GO_OUT }; - enum class Input { PERCEPTION, NONE }; // EXTERNAL: FOA, V2X, etc. struct DebugData { double base_link2front; std::vector, - autoware_auto_perception_msgs::msg::TrafficSignal>> + std::shared_ptr, autoware_perception_msgs::msg::TrafficSignal>> tl_state; std::vector stop_poses; geometry_msgs::msg::Pose first_stop_pose; @@ -77,10 +75,7 @@ class TrafficLightModule : public SceneModuleInterface visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; motion_utils::VirtualWalls createVirtualWalls() override; - inline autoware_auto_perception_msgs::msg::LookingTrafficSignal getTrafficSignal() const - { - return looking_tl_state_; - } + inline TrafficSignal getTrafficSignal() const { return looking_tl_state_; } inline State getTrafficLightModuleState() const { return state_; } @@ -90,10 +85,9 @@ class TrafficLightModule : public SceneModuleInterface } private: - bool isStopSignal(const lanelet::ConstLineStringsOrPolygons3d & traffic_lights); + bool isStopSignal(); - bool isTrafficSignalStop( - const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state) const; + bool isTrafficSignalStop(const TrafficSignal & tl_state) const; autoware_auto_planning_msgs::msg::PathWithLaneId insertStopPose( const autoware_auto_planning_msgs::msg::PathWithLaneId & input, @@ -102,22 +96,13 @@ class TrafficLightModule : public SceneModuleInterface bool isPassthrough(const double & signed_arc_length) const; - bool hasTrafficLightCircleColor( - const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state, - const uint8_t & lamp_color) const; + bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color) const; - bool hasTrafficLightShape( - const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state, - const uint8_t & lamp_shape) const; + bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape) const; - bool getHighestConfidenceTrafficSignal( - const lanelet::ConstLineStringsOrPolygons3d & traffic_lights, - autoware_auto_perception_msgs::msg::TrafficSignalStamped & highest_confidence_tl_state); + bool findValidTrafficSignal(TrafficSignal & valid_traffic_signal); - bool updateTrafficSignal(const lanelet::ConstLineStringsOrPolygons3d & traffic_lights); - - autoware_auto_perception_msgs::msg::TrafficSignalWithJudge generateTlStateWithJudgeFromTlState( - const autoware_auto_perception_msgs::msg::TrafficSignal tl_state) const; + bool updateTrafficSignal(); // Lane id const int64_t lane_id_; @@ -129,9 +114,6 @@ class TrafficLightModule : public SceneModuleInterface // State State state_; - // Input - Input input_; - // Parameter PlannerParam planner_param_; @@ -144,7 +126,7 @@ class TrafficLightModule : public SceneModuleInterface boost::optional first_ref_stop_path_point_index_; // Traffic Light State - autoware_auto_perception_msgs::msg::LookingTrafficSignal looking_tl_state_; + TrafficSignal looking_tl_state_; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_walkway_module/src/manager.cpp b/planning/behavior_velocity_walkway_module/src/manager.cpp index d10ff52850752..f9c222f662e82 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/behavior_velocity_walkway_module/src/manager.cpp @@ -44,11 +44,8 @@ WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node) void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; - if (!opt_use_regulatory_element_) { - opt_use_regulatory_element_ = checkRegulatoryElementExistence(rh->getLaneletMapPtr()); - } - const auto launch = [this, &path](const auto & lanelet) { + const auto launch = [this, &path](const auto & lanelet, const auto use_regulatory_element) { const auto attribute = lanelet.attributeOr(lanelet::AttributeNamesString::Subtype, std::string("")); if (attribute != lanelet::AttributeValueString::Walkway) { @@ -64,24 +61,21 @@ void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); registerModule(std::make_shared( - lanelet.id(), lanelet_map_ptr, p, *opt_use_regulatory_element_, logger, clock_)); + lanelet.id(), lanelet_map_ptr, p, use_regulatory_element, logger, clock_)); }; - if (*opt_use_regulatory_element_) { - const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( - path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); - for (const auto & crosswalk : crosswalk_leg_elem_map) { - launch(crosswalk.first->crosswalkLanelet()); - } - } else { - const auto crosswalk_lanelets = getCrosswalksOnPath( - planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), - rh->getOverallGraphPtr()); + for (const auto & crosswalk : crosswalk_leg_elem_map) { + launch(crosswalk.first->crosswalkLanelet(), true); + } - for (const auto & crosswalk : crosswalk_lanelets) { - launch(crosswalk); - } + const auto crosswalk_lanelets = getCrosswalksOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); + + for (const auto & crosswalk : crosswalk_lanelets) { + launch(crosswalk, false); } } @@ -92,17 +86,14 @@ WalkwayModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) std::set walkway_id_set; - if (*opt_use_regulatory_element_) { - const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( - path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + walkway_id_set = getCrosswalkIdSetOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); - for (const auto & crosswalk : crosswalk_leg_elem_map) { - walkway_id_set.insert(crosswalk.first->id()); - } - } else { - walkway_id_set = getCrosswalkIdSetOnPath( - planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), - rh->getOverallGraphPtr()); + const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + for (const auto & crosswalk : crosswalk_leg_elem_map) { + walkway_id_set.insert(crosswalk.first->id()); } return [walkway_id_set](const std::shared_ptr & scene_module) { diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py index b45357c7bd8b2..9d8fa4a32a93d 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py @@ -93,11 +93,11 @@ def on_timer(self): # traffic signals if traffic_signals_msg: - traffic_signals_msg.header.stamp = timestamp + traffic_signals_msg.stamp = timestamp self.traffic_signals_pub.publish(traffic_signals_msg) self.prev_traffic_signals_msg = traffic_signals_msg elif self.prev_traffic_signals_msg: - self.prev_traffic_signals_msg.header.stamp = timestamp + self.prev_traffic_signals_msg.stamp = timestamp self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) def onPushed(self, event): diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py index a86a9ae9b2bb0..7774a8c82daf6 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py @@ -22,7 +22,7 @@ from autoware_auto_perception_msgs.msg import DetectedObjects from autoware_auto_perception_msgs.msg import PredictedObjects from autoware_auto_perception_msgs.msg import TrackedObjects -from autoware_auto_perception_msgs.msg import TrafficSignalArray +from autoware_perception_msgs.msg import TrafficSignalArray from nav_msgs.msg import Odometry import psutil from rclpy.node import Node diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py index 4228d506d5bec..5344e622be38e 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py @@ -67,11 +67,11 @@ def on_timer(self): # traffic signals if traffic_signals_msg: - traffic_signals_msg.header.stamp = timestamp + traffic_signals_msg.stamp = timestamp self.traffic_signals_pub.publish(traffic_signals_msg) self.prev_traffic_signals_msg = traffic_signals_msg elif self.prev_traffic_signals_msg: - self.prev_traffic_signals_msg.header.stamp = timestamp + self.prev_traffic_signals_msg.stamp = timestamp self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) def find_nearest_ego_odom_by_observation(self, ego_pose): diff --git a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp index ddc304c99ea6a..c5a30be126661 100644 --- a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp +++ b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp @@ -38,11 +38,11 @@ #include #include #include -#include #include #include #include #include +#include #include #include #include @@ -77,10 +77,10 @@ namespace planning_test_utils using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::TrafficSignalArray; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_perception_msgs::msg::TrafficSignalArray; using autoware_planning_msgs::msg::LaneletRoute; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Point; diff --git a/planning/planning_test_utils/package.xml b/planning/planning_test_utils/package.xml index 72662f7ee7b76..23bfb2f44cb96 100644 --- a/planning/planning_test_utils/package.xml +++ b/planning/planning_test_utils/package.xml @@ -15,9 +15,9 @@ autoware_auto_control_msgs autoware_auto_mapping_msgs - autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs + autoware_perception_msgs autoware_planning_msgs component_interface_specs component_interface_utils diff --git a/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml b/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml index 10b5c2814e2a8..5ec10572ffe83 100644 --- a/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml +++ b/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml @@ -1,12 +1,61 @@ /**: ros__parameters: - # obstacle check - use_pointcloud: true # use pointcloud as obstacle check - use_dynamic_object: true # use dynamic object as obstacle check - surround_check_distance: 0.5 # if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m] - surround_check_recover_distance: 0.8 # if no object exists in this distance, transit to "non-surrounding-obstacle" status [m] + # surround_check_*_distance: if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m] + # surround_check_hysteresis_distance: if no object exists in this hysteresis distance added to the above distance, transit to "non-surrounding-obstacle" status [m] + pointcloud: + enable_check: false + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.5 + surround_check_back_distance: 0.5 + unknown: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.5 + surround_check_back_distance: 0.5 + car: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + surround_check_back_distance: 0.5 + truck: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + surround_check_back_distance: 0.5 + bus: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + surround_check_back_distance: 0.5 + trailer: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + surround_check_back_distance: 0.5 + motorcycle: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + surround_check_back_distance: 0.5 + bicycle: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.5 + surround_check_back_distance: 0.5 + pedestrian: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.5 + surround_check_back_distance: 0.5 + + surround_check_hysteresis_distance: 0.3 + state_clear_time: 2.0 + # ego stop state + stop_state_ego_speed: 0.1 #[m/s] + # debug publish_debug_footprints: true # publish vehicle footprint & footprints with surround_check_distance and surround_check_recover_distance offsets + debug_footprint_label: "car" diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp index bd0fe48bbda28..590f026942801 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp @@ -16,6 +16,7 @@ #define SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ #include +#include #include #include @@ -38,6 +39,7 @@ using geometry_msgs::msg::PolygonStamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; using tier4_planning_msgs::msg::StopReasonArray; +using vehicle_info_util::VehicleInfo; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -52,15 +54,19 @@ class SurroundObstacleCheckerDebugNode { public: explicit SurroundObstacleCheckerDebugNode( - const Polygon2d & ego_polygon, const double base_link2front, - const double & surround_check_distance, const double & surround_check_recover_distance, - const geometry_msgs::msg::Pose & self_pose, const rclcpp::Clock::SharedPtr clock, - rclcpp::Node & node); + const vehicle_info_util::VehicleInfo & vehicle_info, const double base_link2front, + const std::string & object_label, const double & surround_check_front_distance, + const double & surround_check_side_distance, const double & surround_check_back_distance, + const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose, + const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node); bool pushPose(const geometry_msgs::msg::Pose & pose, const PoseType & type); bool pushObstaclePoint(const geometry_msgs::msg::Point & obstacle_point, const PointType & type); void publish(); void publishFootprints(); + void updateFootprintMargin( + const std::string & object_label, const double front_distance, const double side_distance, + const double back_distance); private: rclcpp::Publisher::SharedPtr debug_virtual_wall_pub_; @@ -72,10 +78,13 @@ class SurroundObstacleCheckerDebugNode rclcpp::Publisher::SharedPtr vehicle_footprint_offset_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_recover_offset_pub_; - Polygon2d ego_polygon_; + vehicle_info_util::VehicleInfo vehicle_info_; double base_link2front_; - double surround_check_distance_; - double surround_check_recover_distance_; + std::string object_label_; + double surround_check_front_distance_; + double surround_check_side_distance_; + double surround_check_back_distance_; + double surround_check_hysteresis_distance_; geometry_msgs::msg::Pose self_pose_; MarkerArray makeVirtualWallMarker(); @@ -83,7 +92,6 @@ class SurroundObstacleCheckerDebugNode StopReasonArray makeStopReasonArray(); VelocityFactorArray makeVelocityFactorArray(); - Polygon2d createSelfPolygonWithOffset(const Polygon2d & base_polygon, const double & offset); PolygonStamped boostPolygonToPolygonStamped(const Polygon2d & boost_polygon, const double & z); std::shared_ptr stop_obstacle_point_ptr_; diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp index de3e4bf120b13..a7c177f202173 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp @@ -38,6 +38,7 @@ #include #include +#include #include #include @@ -62,18 +63,27 @@ class SurroundObstacleCheckerNode : public rclcpp::Node struct NodeParam { - bool use_pointcloud; - bool use_dynamic_object; bool is_surround_obstacle; - // For preventing chattering, - // surround_check_recover_distance_ must be bigger than surround_check_distance_ - double surround_check_recover_distance; - double surround_check_distance; + std::unordered_map enable_check_map; + std::unordered_map surround_check_front_distance_map; + std::unordered_map surround_check_side_distance_map; + std::unordered_map surround_check_back_distance_map; + bool pointcloud_enable_check; + double pointcloud_surround_check_front_distance; + double pointcloud_surround_check_side_distance; + double pointcloud_surround_check_back_distance; + double surround_check_hysteresis_distance; double state_clear_time; bool publish_debug_footprints; + std::string debug_footprint_label; }; private: + rcl_interfaces::msg::SetParametersResult onParam( + const std::vector & parameters); + + std::array getCheckDistances(const std::string & str_label) const; + void onTimer(); void onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); @@ -107,6 +117,9 @@ class SurroundObstacleCheckerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; rclcpp::Publisher::SharedPtr pub_velocity_limit_; + // parameter callback result + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + // stop checker std::unique_ptr vehicle_stop_checker_; diff --git a/planning/surround_obstacle_checker/src/debug_marker.cpp b/planning/surround_obstacle_checker/src/debug_marker.cpp index c30f778584fd7..79c2300be3941 100644 --- a/planning/surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/surround_obstacle_checker/src/debug_marker.cpp @@ -26,6 +26,29 @@ namespace surround_obstacle_checker { +namespace +{ +Polygon2d createSelfPolygon( + const VehicleInfo & vehicle_info, const double front_margin = 0.0, const double side_margin = 0.0, + const double rear_margin = 0.0) +{ + const double & front_m = vehicle_info.max_longitudinal_offset_m + front_margin; + const double & width_left_m = vehicle_info.max_lateral_offset_m + side_margin; + const double & width_right_m = vehicle_info.min_lateral_offset_m - side_margin; + const double & rear_m = vehicle_info.min_longitudinal_offset_m - rear_margin; + + Polygon2d ego_polygon; + + ego_polygon.outer().push_back(Point2d(front_m, width_left_m)); + ego_polygon.outer().push_back(Point2d(front_m, width_right_m)); + ego_polygon.outer().push_back(Point2d(rear_m, width_right_m)); + ego_polygon.outer().push_back(Point2d(rear_m, width_left_m)); + + bg::correct(ego_polygon); + + return ego_polygon; +} +} // namespace using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::appendMarkerArray; @@ -36,14 +59,18 @@ using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( - const Polygon2d & ego_polygon, const double base_link2front, - const double & surround_check_distance, const double & surround_check_recover_distance, - const geometry_msgs::msg::Pose & self_pose, const rclcpp::Clock::SharedPtr clock, - rclcpp::Node & node) -: ego_polygon_(ego_polygon), + const vehicle_info_util::VehicleInfo & vehicle_info, const double base_link2front, + const std::string & object_label, const double & surround_check_front_distance, + const double & surround_check_side_distance, const double & surround_check_back_distance, + const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose, + const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node) +: vehicle_info_(vehicle_info), base_link2front_(base_link2front), - surround_check_distance_(surround_check_distance), - surround_check_recover_distance_(surround_check_recover_distance), + object_label_(object_label), + surround_check_front_distance_(surround_check_front_distance), + surround_check_side_distance_(surround_check_side_distance), + surround_check_back_distance_(surround_check_back_distance), + surround_check_hysteresis_distance_(surround_check_hysteresis_distance), self_pose_(self_pose), clock_(clock) { @@ -86,20 +113,25 @@ bool SurroundObstacleCheckerDebugNode::pushObstaclePoint( void SurroundObstacleCheckerDebugNode::publishFootprints() { + const auto ego_polygon = createSelfPolygon(vehicle_info_); + /* publish vehicle footprint polygon */ - const auto footprint = boostPolygonToPolygonStamped(ego_polygon_, self_pose_.position.z); + const auto footprint = boostPolygonToPolygonStamped(ego_polygon, self_pose_.position.z); vehicle_footprint_pub_->publish(footprint); /* publish vehicle footprint polygon with offset */ - const auto polygon_with_offset = - createSelfPolygonWithOffset(ego_polygon_, surround_check_distance_); + const auto polygon_with_offset = createSelfPolygon( + vehicle_info_, surround_check_front_distance_, surround_check_side_distance_, + surround_check_back_distance_); const auto footprint_with_offset = boostPolygonToPolygonStamped(polygon_with_offset, self_pose_.position.z); vehicle_footprint_offset_pub_->publish(footprint_with_offset); /* publish vehicle footprint polygon with recover offset */ - const auto polygon_with_recover_offset = - createSelfPolygonWithOffset(ego_polygon_, surround_check_recover_distance_); + const auto polygon_with_recover_offset = createSelfPolygon( + vehicle_info_, surround_check_front_distance_ + surround_check_hysteresis_distance_, + surround_check_side_distance_ + surround_check_hysteresis_distance_, + surround_check_back_distance_ + surround_check_hysteresis_distance_); const auto footprint_with_recover_offset = boostPolygonToPolygonStamped(polygon_with_recover_offset, self_pose_.position.z); vehicle_footprint_recover_offset_pub_->publish(footprint_with_recover_offset); @@ -206,26 +238,6 @@ VelocityFactorArray SurroundObstacleCheckerDebugNode::makeVelocityFactorArray() return velocity_factor_array; } -Polygon2d SurroundObstacleCheckerDebugNode::createSelfPolygonWithOffset( - const Polygon2d & base_polygon, const double & offset) -{ - typedef double coordinate_type; - const double buffer_distance = offset; - const int points_per_circle = 36; - boost::geometry::strategy::buffer::distance_symmetric distance_strategy( - buffer_distance); - boost::geometry::strategy::buffer::join_round join_strategy(points_per_circle); - boost::geometry::strategy::buffer::end_round end_strategy(points_per_circle); - boost::geometry::strategy::buffer::point_circle circle_strategy(points_per_circle); - boost::geometry::strategy::buffer::side_straight side_strategy; - boost::geometry::model::multi_polygon result; - // Create the buffer of a multi polygon - boost::geometry::buffer( - base_polygon, result, distance_strategy, side_strategy, join_strategy, end_strategy, - circle_strategy); - return result.front(); -} - PolygonStamped SurroundObstacleCheckerDebugNode::boostPolygonToPolygonStamped( const Polygon2d & boost_polygon, const double & z) { @@ -244,4 +256,14 @@ PolygonStamped SurroundObstacleCheckerDebugNode::boostPolygonToPolygonStamped( return polygon_stamped; } +void SurroundObstacleCheckerDebugNode::updateFootprintMargin( + const std::string & object_label, const double front_distance, const double side_distance, + const double back_distance) +{ + object_label_ = object_label; + surround_check_front_distance_ = front_distance; + surround_check_side_distance_ = side_distance; + surround_check_back_distance_ = back_distance; +} + } // namespace surround_obstacle_checker diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index 17a47be8c0596..4ea3e81d65411 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -47,6 +47,7 @@ namespace surround_obstacle_checker namespace bg = boost::geometry; using Point2d = bg::model::d2::point_xy; using Polygon2d = bg::model::polygon; +using autoware_auto_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::pose2transform; @@ -120,12 +121,14 @@ Polygon2d createObjPolygon( return createObjPolygon(pose, polygon); } -Polygon2d createSelfPolygon(const VehicleInfo & vehicle_info) +Polygon2d createSelfPolygon( + const VehicleInfo & vehicle_info, const double front_margin, const double side_margin, + const double rear_margin) { - const double & front_m = vehicle_info.max_longitudinal_offset_m; - const double & width_left_m = vehicle_info.max_lateral_offset_m; - const double & width_right_m = vehicle_info.min_lateral_offset_m; - const double & rear_m = vehicle_info.min_longitudinal_offset_m; + const double & front_m = vehicle_info.max_longitudinal_offset_m + front_margin; + const double & width_left_m = vehicle_info.max_lateral_offset_m + side_margin; + const double & width_right_m = vehicle_info.min_lateral_offset_m - side_margin; + const double & rear_m = vehicle_info.min_longitudinal_offset_m - rear_margin; Polygon2d ego_polygon; @@ -146,13 +149,42 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio // Parameters { auto & p = node_param_; - p.use_pointcloud = this->declare_parameter("use_pointcloud"); - p.use_dynamic_object = this->declare_parameter("use_dynamic_object"); - p.surround_check_distance = this->declare_parameter("surround_check_distance"); - p.surround_check_recover_distance = - this->declare_parameter("surround_check_recover_distance"); + + // for object label + std::unordered_map label_map{ + {"unknown", ObjectClassification::UNKNOWN}, {"car", ObjectClassification::CAR}, + {"truck", ObjectClassification::TRUCK}, {"bus", ObjectClassification::BUS}, + {"trailer", ObjectClassification::TRAILER}, {"motorcycle", ObjectClassification::MOTORCYCLE}, + {"bicycle", ObjectClassification::BICYCLE}, {"pedestrian", ObjectClassification::PEDESTRIAN}}; + for (const auto & label_pair : label_map) { + p.enable_check_map.emplace( + label_pair.second, this->declare_parameter(label_pair.first + ".enable_check")); + p.surround_check_front_distance_map.emplace( + label_pair.second, + this->declare_parameter(label_pair.first + ".surround_check_front_distance")); + p.surround_check_side_distance_map.emplace( + label_pair.second, + this->declare_parameter(label_pair.first + ".surround_check_side_distance")); + p.surround_check_back_distance_map.emplace( + label_pair.second, + this->declare_parameter(label_pair.first + ".surround_check_back_distance")); + } + + // for pointcloud + p.pointcloud_enable_check = this->declare_parameter("pointcloud.enable_check"); + p.pointcloud_surround_check_front_distance = + this->declare_parameter("pointcloud.surround_check_front_distance"); + p.pointcloud_surround_check_side_distance = + this->declare_parameter("pointcloud.surround_check_side_distance"); + p.pointcloud_surround_check_back_distance = + this->declare_parameter("pointcloud.surround_check_back_distance"); + + p.surround_check_hysteresis_distance = + this->declare_parameter("surround_check_hysteresis_distance"); + p.state_clear_time = this->declare_parameter("state_clear_time"); p.publish_debug_footprints = this->declare_parameter("publish_debug_footprints"); + p.debug_footprint_label = this->declare_parameter("debug_footprint_label"); } vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -176,6 +208,10 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio "~/input/odometry", 1, std::bind(&SurroundObstacleCheckerNode::onOdometry, this, std::placeholders::_1)); + // Parameter callback + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&SurroundObstacleCheckerNode::onParam, this, std::placeholders::_1)); + using std::chrono_literals::operator""ms; timer_ = rclcpp::create_timer( this, get_clock(), 100ms, std::bind(&SurroundObstacleCheckerNode::onTimer, this)); @@ -184,15 +220,55 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio vehicle_stop_checker_ = std::make_unique(this); // Debug - auto const self_polygon = createSelfPolygon(vehicle_info_); odometry_ptr_ = std::make_shared(); + const auto check_distances = getCheckDistances(node_param_.debug_footprint_label); debug_ptr_ = std::make_shared( - self_polygon, vehicle_info_.max_longitudinal_offset_m, node_param_.surround_check_distance, - node_param_.surround_check_recover_distance, odometry_ptr_->pose.pose, this->get_clock(), + vehicle_info_, vehicle_info_.max_longitudinal_offset_m, node_param_.debug_footprint_label, + check_distances.at(0), check_distances.at(1), check_distances.at(2), + node_param_.surround_check_hysteresis_distance, odometry_ptr_->pose.pose, this->get_clock(), *this); } +std::array SurroundObstacleCheckerNode::getCheckDistances( + const std::string & str_label) const +{ + if (str_label == "pointcloud") { + return { + node_param_.pointcloud_surround_check_front_distance, + node_param_.pointcloud_surround_check_side_distance, + node_param_.pointcloud_surround_check_back_distance}; + } + + std::unordered_map label_map{ + {"unknown", ObjectClassification::UNKNOWN}, {"car", ObjectClassification::CAR}, + {"truck", ObjectClassification::TRUCK}, {"bus", ObjectClassification::BUS}, + {"trailer", ObjectClassification::TRAILER}, {"motorcycle", ObjectClassification::MOTORCYCLE}, + {"bicycle", ObjectClassification::BICYCLE}, {"pedestrian", ObjectClassification::PEDESTRIAN}}; + + const int int_label = label_map.at(str_label); + return { + node_param_.surround_check_front_distance_map.at(int_label), + node_param_.surround_check_side_distance_map.at(int_label), + node_param_.surround_check_back_distance_map.at(int_label)}; +} + +rcl_interfaces::msg::SetParametersResult SurroundObstacleCheckerNode::onParam( + const std::vector & parameters) +{ + tier4_autoware_utils::updateParam( + parameters, "debug_footprint_label", node_param_.debug_footprint_label); + const auto check_distances = getCheckDistances(node_param_.debug_footprint_label); + debug_ptr_->updateFootprintMargin( + node_param_.debug_footprint_label, check_distances.at(0), check_distances.at(1), + check_distances.at(2)); + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} + void SurroundObstacleCheckerNode::onTimer() { if (!odometry_ptr_) { @@ -205,13 +281,22 @@ void SurroundObstacleCheckerNode::onTimer() debug_ptr_->publishFootprints(); } - if (node_param_.use_pointcloud && !pointcloud_ptr_) { + if (node_param_.pointcloud_enable_check && !pointcloud_ptr_) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for pointcloud info..."); return; } - if (node_param_.use_dynamic_object && !object_ptr_) { + const bool use_dynamic_object = + node_param_.enable_check_map.at(ObjectClassification::UNKNOWN) || + node_param_.enable_check_map.at(ObjectClassification::CAR) || + node_param_.enable_check_map.at(ObjectClassification::TRUCK) || + node_param_.enable_check_map.at(ObjectClassification::BUS) || + node_param_.enable_check_map.at(ObjectClassification::TRAILER) || + node_param_.enable_check_map.at(ObjectClassification::MOTORCYCLE) || + node_param_.enable_check_map.at(ObjectClassification::BICYCLE) || + node_param_.enable_check_map.at(ObjectClassification::PEDESTRIAN); + if (use_dynamic_object && !object_ptr_) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for dynamic object info..."); return; @@ -220,11 +305,11 @@ void SurroundObstacleCheckerNode::onTimer() const auto nearest_obstacle = getNearestObstacle(); const auto is_vehicle_stopped = vehicle_stop_checker_->isVehicleStopped(); + constexpr double epsilon = 1e-3; switch (state_) { case State::PASS: { const auto is_obstacle_found = - !nearest_obstacle ? false - : nearest_obstacle.get().first < node_param_.surround_check_distance; + !nearest_obstacle ? false : nearest_obstacle.get().first < epsilon; if (!isStopRequired(is_obstacle_found, is_vehicle_stopped)) { break; @@ -250,7 +335,7 @@ void SurroundObstacleCheckerNode::onTimer() const auto is_obstacle_found = !nearest_obstacle ? false - : nearest_obstacle.get().first < node_param_.surround_check_recover_distance; + : nearest_obstacle.get().first < node_param_.surround_check_hysteresis_distance; if (isStopRequired(is_obstacle_found, is_vehicle_stopped)) { break; @@ -304,17 +389,8 @@ void SurroundObstacleCheckerNode::onOdometry(const nav_msgs::msg::Odometry::Cons boost::optional SurroundObstacleCheckerNode::getNearestObstacle() const { - boost::optional nearest_pointcloud{boost::none}; - boost::optional nearest_object{boost::none}; - - if (node_param_.use_pointcloud) { - nearest_pointcloud = getNearestObstacleByPointCloud(); - } - - if (node_param_.use_dynamic_object) { - nearest_object = getNearestObstacleByDynamicObject(); - } - + const auto nearest_pointcloud = getNearestObstacleByPointCloud(); + const auto nearest_object = getNearestObstacleByDynamicObject(); if (!nearest_pointcloud && !nearest_object) { return {}; } @@ -333,15 +409,13 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacle() cons boost::optional SurroundObstacleCheckerNode::getNearestObstacleByPointCloud() const { - if (pointcloud_ptr_->data.empty()) { + if (!node_param_.pointcloud_enable_check || pointcloud_ptr_->data.empty()) { return boost::none; } + const auto transform_stamped = getTransform("base_link", pointcloud_ptr_->header.frame_id, pointcloud_ptr_->header.stamp, 0.5); - geometry_msgs::msg::Point nearest_point; - auto minimum_distance = std::numeric_limits::max(); - if (!transform_stamped) { return {}; } @@ -352,8 +426,14 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByPoint tier4_autoware_utils::transformPointCloud( transformed_pointcloud, transformed_pointcloud, isometry); - const auto ego_polygon = createSelfPolygon(vehicle_info_); + const double front_margin = node_param_.pointcloud_surround_check_front_distance; + const double side_margin = node_param_.pointcloud_surround_check_side_distance; + const double back_margin = node_param_.pointcloud_surround_check_back_distance; + const auto ego_polygon = createSelfPolygon(vehicle_info_, front_margin, side_margin, back_margin); + geometry_msgs::msg::Point nearest_point; + double minimum_distance = std::numeric_limits::max(); + bool was_minimum_distance_updated = false; for (const auto & p : transformed_pointcloud) { Point2d boost_point(p.x, p.y); @@ -362,10 +442,14 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByPoint if (distance_to_object < minimum_distance) { nearest_point = createPoint(p.x, p.y, p.z); minimum_distance = distance_to_object; + was_minimum_distance_updated = true; } } - return std::make_pair(minimum_distance, nearest_point); + if (was_minimum_distance_updated) { + return std::make_pair(minimum_distance, nearest_point); + } + return boost::none; } boost::optional SurroundObstacleCheckerNode::getNearestObstacleByDynamicObject() const @@ -373,9 +457,6 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByDynam const auto transform_stamped = getTransform(object_ptr_->header.frame_id, "base_link", object_ptr_->header.stamp, 0.5); - geometry_msgs::msg::Point nearest_point; - auto minimum_distance = std::numeric_limits::max(); - if (!transform_stamped) { return {}; } @@ -383,10 +464,23 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByDynam tf2::Transform tf_src2target; tf2::fromMsg(transform_stamped.get().transform, tf_src2target); - const auto ego_polygon = createSelfPolygon(vehicle_info_); - + // TODO(murooka) check computation cost + geometry_msgs::msg::Point nearest_point; + double minimum_distance = std::numeric_limits::max(); + bool was_minimum_distance_updated = false; for (const auto & object : object_ptr_->objects) { const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; + const int label = object.classification.front().label; + + if (!node_param_.enable_check_map.at(label)) { + continue; + } + + const double front_margin = node_param_.surround_check_front_distance_map.at(label); + const double side_margin = node_param_.surround_check_side_distance_map.at(label); + const double back_margin = node_param_.surround_check_back_distance_map.at(label); + const auto ego_polygon = + createSelfPolygon(vehicle_info_, front_margin, side_margin, back_margin); tf2::Transform tf_src2object; tf2::fromMsg(object_pose, tf_src2object); @@ -404,10 +498,14 @@ boost::optional SurroundObstacleCheckerNode::getNearestObstacleByDynam if (distance_to_object < minimum_distance) { nearest_point = object_pose.position; minimum_distance = distance_to_object; + was_minimum_distance_updated = true; } } - return std::make_pair(minimum_distance, nearest_point); + if (was_minimum_distance_updated) { + return std::make_pair(minimum_distance, nearest_point); + } + return boost::none; } boost::optional SurroundObstacleCheckerNode::getTransform( 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 (