diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 091f3405e2815..1a800a38d1051 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -109,13 +109,16 @@ std::vector resamplePointVector( } std::vector resamplePoseVector( - const std::vector & points, + const std::vector & points_raw, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z) { + // Remove overlap points for resampling + const auto points = motion_utils::removeOverlapPoints(points_raw); + // validate arguments if (!resample_utils::validate_arguments(points, resampled_arclength)) { - return points; + return points_raw; } std::vector position(points.size()); diff --git a/common/object_recognition_utils/include/object_recognition_utils/transform.hpp b/common/object_recognition_utils/include/object_recognition_utils/transform.hpp index 4446c87427e88..31892853a855f 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/transform.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/transform.hpp @@ -15,15 +15,22 @@ #ifndef OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ #define OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ +#include + #include +#include +#include #include #include #include #ifdef ROS_DISTRO_GALACTIC +#include #include #else +#include + #include #endif @@ -45,6 +52,23 @@ namespace detail return boost::none; } } + +[[maybe_unused]] inline boost::optional getTransformMatrix( + const std::string & in_target_frame, const std_msgs::msg::Header & in_cloud_header, + const tf2_ros::Buffer & tf_buffer) +{ + try { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped = tf_buffer.lookupTransform( + in_target_frame, in_cloud_header.frame_id, in_cloud_header.stamp, + rclcpp::Duration::from_seconds(1.0)); + Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + return mat; + } catch (tf2::TransformException & e) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("detail::getTransformMatrix"), e.what()); + return boost::none; + } +} } // namespace detail namespace object_recognition_utils @@ -79,6 +103,46 @@ bool transformObjects( } return true; } +template +bool transformObjectsWithFeature( + const T & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, + T & output_msg) +{ + output_msg = input_msg; + if (input_msg.header.frame_id != target_frame_id) { + output_msg.header.frame_id = target_frame_id; + tf2::Transform tf_target2objects_world; + tf2::Transform tf_target2objects; + tf2::Transform tf_objects_world2objects; + const auto ros_target2objects_world = detail::getTransform( + tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); + if (!ros_target2objects_world) { + return false; + } + const auto tf_matrix = detail::getTransformMatrix(target_frame_id, input_msg.header, tf_buffer); + if (!tf_matrix) { + RCLCPP_WARN( + rclcpp::get_logger("object_recognition_utils:"), "failed to get transformed matrix"); + return false; + } + for (auto & feature_object : output_msg.feature_objects) { + // transform object + tf2::fromMsg( + feature_object.object.kinematics.pose_with_covariance.pose, tf_objects_world2objects); + tf_target2objects = tf_target2objects_world * tf_objects_world2objects; + tf2::toMsg(tf_target2objects, feature_object.object.kinematics.pose_with_covariance.pose); + + // transform cluster + sensor_msgs::msg::PointCloud2 transformed_cluster; + pcl_ros::transformPointCloud(*tf_matrix, feature_object.feature.cluster, transformed_cluster); + transformed_cluster.header.frame_id = target_frame_id; + feature_object.feature.cluster = transformed_cluster; + } + output_msg.header.frame_id = target_frame_id; + return true; + } + return true; +} } // namespace object_recognition_utils #endif // OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ diff --git a/common/object_recognition_utils/package.xml b/common/object_recognition_utils/package.xml index 95925e846a55c..2f2472515ebad 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -17,7 +17,13 @@ geometry_msgs interpolation libboost-dev + pcl_conversions + pcl_ros rclcpp + sensor_msgs + std_msgs + tf2 + tf2_eigen tier4_autoware_utils ament_cmake_ros diff --git a/common/perception_utils/package.xml b/common/perception_utils/package.xml index 6cae7f9ce8c4f..5304d481737e5 100644 --- a/common/perception_utils/package.xml +++ b/common/perception_utils/package.xml @@ -4,7 +4,6 @@ perception_utils 0.1.0 The perception_utils package - Mingyu Li Satoshi Tanaka Yusuke Muramatsu Shunsuke Miura diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp index ff448e694eb15..058a5d5deb5d6 100644 --- a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp @@ -366,7 +366,10 @@ void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg rtc_table_->clearContents(); num_rtc_status_ptr_->setText( QString::fromStdString("The Number of RTC Statuses: " + std::to_string(msg->statuses.size()))); - if (msg->statuses.empty()) return; + if (msg->statuses.empty()) { + rtc_table_->update(); + return; + } // this is to stable rtc display not to occupy too much size_t min_display_size{5}; size_t max_display_size{10}; @@ -374,8 +377,17 @@ void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg rtc_table_->setRowCount( std::max(min_display_size, std::min(msg->statuses.size(), max_display_size))); int cnt = 0; - for (auto status : msg->statuses) { - if (static_cast(cnt) >= max_display_size) return; + + auto sorted_statuses = msg->statuses; + std::partition(sorted_statuses.begin(), sorted_statuses.end(), [](const auto & status) { + return !status.auto_mode && !uint2bool(status.command_status.type); + }); + + for (auto status : sorted_statuses) { + if (static_cast(cnt) >= max_display_size) { + rtc_table_->update(); + return; + } // uuid { std::stringstream uuid; 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/control_validator.cpp b/control/control_validator/src/control_validator.cpp index 9fe5121ee3dbd..6c3996bfd274b 100644 --- a/control/control_validator/src/control_validator.cpp +++ b/control/control_validator/src/control_validator.cpp @@ -163,7 +163,9 @@ void ControlValidator::publishDebugInfo() void ControlValidator::validate(const Trajectory & predicted_trajectory) { if (predicted_trajectory.points.size() < 2) { - RCLCPP_ERROR(get_logger(), "predicted_trajectory size is less than 2. Cannot validate."); + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, + "predicted_trajectory size is less than 2. Cannot validate."); return; } diff --git a/control/lane_departure_checker/README.md b/control/lane_departure_checker/README.md index 5148d6a998f50..67b770aefb3d2 100644 --- a/control/lane_departure_checker/README.md +++ b/control/lane_departure_checker/README.md @@ -63,15 +63,30 @@ This package includes the following features: ### Node Parameters -| Name | Type | Description | Default value | -| :---------------------------- | :----- | :------------------------------------------------------------ | :------------ | -| update_rate | double | Frequency for publishing [Hz] | 10.0 | -| visualize_lanelet | bool | Flag for visualizing lanelet | False | -| include_right_lanes | bool | Flag for including right lanelet in borders | False | -| include_left_lanes | bool | Flag for including left lanelet in borders | False | -| include_opposite_lanes | bool | Flag for including opposite lanelet in borders | False | -| include_conflicting_lanes | bool | Flag for including conflicting lanelet in borders | False | -| road_border_departure_checker | bool | Flag for checking if the vehicle will departs the road border | False | +#### General Parameters + +| Name | Type | Description | Default value | +| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :------------ | +| will_out_of_lane_checker | bool | Enable checker whether ego vehicle footprint will depart from lane | True | +| out_of_lane_checker | bool | Enable checker whether ego vehicle footprint is out of lane | True | +| boundary_departure_checker | bool | Enable checker whether ego vehicle footprint wil depart from boundary specified by boundary_types_to_detect | False | +| update_rate | double | Frequency for publishing [Hz] | 10.0 | +| visualize_lanelet | bool | Flag for visualizing lanelet | False | + +#### Parameters For Lane Departure + +| Name | Type | Description | Default value | +| :------------------------ | :--- | :------------------------------------------------ | :------------ | +| include_right_lanes | bool | Flag for including right lanelet in borders | False | +| include_left_lanes | bool | Flag for including left lanelet in borders | False | +| include_opposite_lanes | bool | Flag for including opposite lanelet in borders | False | +| include_conflicting_lanes | bool | Flag for including conflicting lanelet in borders | False | + +#### Parameters For Road Border Departure + +| Name | Type | Description | Default value | +| :----------------------- | :------------------------- | :---------------------------------------------------------- | :------------ | +| boundary_types_to_detect | std::vector\ | line_string types to detect with boundary_departure_checker | [road_border] | ### Core Parameters diff --git a/control/lane_departure_checker/config/lane_departure_checker.param.yaml b/control/lane_departure_checker/config/lane_departure_checker.param.yaml index 008832b1cab21..f0a8df21c425b 100644 --- a/control/lane_departure_checker/config/lane_departure_checker.param.yaml +++ b/control/lane_departure_checker/config/lane_departure_checker.param.yaml @@ -1,5 +1,10 @@ /**: ros__parameters: + # Enable feature + will_out_of_lane_checker: true + out_of_lane_checker: true + boundary_departure_checker: false + # Node update_rate: 10.0 visualize_lanelet: false @@ -7,7 +12,8 @@ include_left_lanes: false include_opposite_lanes: false include_conflicting_lanes: false - road_border_departure_checker: false + boundary_types_to_detect: [road_border] + # Core footprint_margin_scale: 1.0 diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index 573a38593fc59..c77503106e485 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -72,6 +72,7 @@ struct Input lanelet::ConstLanelets shoulder_lanelets{}; Trajectory::ConstSharedPtr reference_trajectory{}; Trajectory::ConstSharedPtr predicted_trajectory{}; + std::vector boundary_types_to_detect{}; }; struct Output @@ -79,7 +80,7 @@ struct Output std::map processing_time_map{}; bool will_leave_lane{}; bool is_out_of_lane{}; - bool will_cross_road_border{}; + bool will_cross_boundary{}; PoseDeviation trajectory_deviation{}; lanelet::ConstLanelets candidate_lanelets{}; TrajectoryPoints resampled_trajectory{}; @@ -136,9 +137,10 @@ class LaneDepartureChecker const lanelet::ConstLanelets & candidate_lanelets, const std::vector & vehicle_footprints); - static bool willCrossRoadBorder( + static bool willCrossBoundary( const lanelet::ConstLanelets & candidate_lanelets, - const std::vector & vehicle_footprints); + const std::vector & vehicle_footprints, + const std::vector & boundary_types_to_detects); static bool isCrossingRoadBorder( const lanelet::BasicLineString2d & road_border, const std::vector & footprints); diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp index f7f7bcfda7d51..dd05ab226f6b7 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp @@ -18,8 +18,6 @@ #include "lane_departure_checker/lane_departure_checker.hpp" #include -#include -#include #include #include #include @@ -34,9 +32,12 @@ #include #include +#include +#include #include #include +#include #include namespace lane_departure_checker @@ -45,13 +46,17 @@ using autoware_auto_mapping_msgs::msg::HADMapBin; struct NodeParam { + bool will_out_of_lane_checker; + bool out_of_lane_checker; + bool boundary_departure_checker; + double update_rate; bool visualize_lanelet; bool include_right_lanes; bool include_left_lanes; bool include_opposite_lanes; bool include_conflicting_lanes; - bool road_border_departure_checker; + std::vector boundary_types_to_detect; }; class LaneDepartureCheckerNode : public rclcpp::Node diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index f1d8d75452df1..9d413b2bf0a70 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -67,19 +67,19 @@ bool isInAnyLane(const lanelet::ConstLanelets & candidate_lanelets, const Point2 return false; } -bool isCrossingWithRoadBorder( - const lanelet::BasicLineString2d & road_border, const std::vector & footprints) +bool isCrossingWithBoundary( + const lanelet::BasicLineString2d & boundary, const std::vector & footprints) { for (auto & footprint : footprints) { for (size_t i = 0; i < footprint.size() - 1; ++i) { auto footprint1 = footprint.at(i).to_3d(); auto footprint2 = footprint.at(i + 1).to_3d(); - for (size_t i = 0; i < road_border.size() - 1; ++i) { - auto road_border1 = road_border.at(i); - auto road_border2 = road_border.at(i + 1); + for (size_t i = 0; i < boundary.size() - 1; ++i) { + auto boundary1 = boundary.at(i); + auto boundary2 = boundary.at(i + 1); if (tier4_autoware_utils::intersect( tier4_autoware_utils::toMsg(footprint1), tier4_autoware_utils::toMsg(footprint2), - fromVector2dToMsg(road_border1), fromVector2dToMsg(road_border2))) { + fromVector2dToMsg(boundary1), fromVector2dToMsg(boundary2))) { return true; } } @@ -172,9 +172,9 @@ Output LaneDepartureChecker::update(const Input & input) output.is_out_of_lane = isOutOfLane(output.candidate_lanelets, output.vehicle_footprints.front()); output.processing_time_map["isOutOfLane"] = stop_watch.toc(true); - output.will_cross_road_border = - willCrossRoadBorder(output.candidate_lanelets, output.vehicle_footprints); - output.processing_time_map["willCrossRoadBorder"] = stop_watch.toc(true); + output.will_cross_boundary = willCrossBoundary( + output.candidate_lanelets, output.vehicle_footprints, input.boundary_types_to_detect); + output.processing_time_map["willCrossBoundary"] = stop_watch.toc(true); return output; } @@ -334,15 +334,18 @@ bool LaneDepartureChecker::isOutOfLane( return false; } -bool LaneDepartureChecker::willCrossRoadBorder( +bool LaneDepartureChecker::willCrossBoundary( const lanelet::ConstLanelets & candidate_lanelets, - const std::vector & vehicle_footprints) + const std::vector & vehicle_footprints, + const std::vector & boundary_types_to_detect) { for (const auto & candidate_lanelet : candidate_lanelets) { const std::string r_type = candidate_lanelet.rightBound().attributeOr(lanelet::AttributeName::Type, "none"); - if (r_type == "road_border") { - if (isCrossingWithRoadBorder( + if ( + std::find(boundary_types_to_detect.begin(), boundary_types_to_detect.end(), r_type) != + boundary_types_to_detect.end()) { + if (isCrossingWithBoundary( candidate_lanelet.rightBound2d().basicLineString(), vehicle_footprints)) { // std::cerr << "The crossed road_border's line string id: " // << candidate_lanelet.rightBound().id() << std::endl; @@ -351,8 +354,10 @@ bool LaneDepartureChecker::willCrossRoadBorder( } const std::string l_type = candidate_lanelet.leftBound().attributeOr(lanelet::AttributeName::Type, "none"); - if (l_type == "road_border") { - if (isCrossingWithRoadBorder( + if ( + std::find(boundary_types_to_detect.begin(), boundary_types_to_detect.end(), l_type) != + boundary_types_to_detect.end()) { + if (isCrossingWithBoundary( candidate_lanelet.leftBound2d().basicLineString(), vehicle_footprints)) { // std::cerr << "The crossed road_border's line string id: " // << candidate_lanelet.leftBound().id() << std::endl; diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 20d535a82bfa1..b40a0d0473135 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -14,8 +14,10 @@ #include "lane_departure_checker/lane_departure_checker_node.hpp" +#include #include #include +#include #include #include #include @@ -125,6 +127,11 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o { using std::placeholders::_1; + // Enable feature + node_param_.will_out_of_lane_checker = declare_parameter("will_out_of_lane_checker"); + node_param_.out_of_lane_checker = declare_parameter("out_of_lane_checker"); + node_param_.boundary_departure_checker = declare_parameter("boundary_departure_checker"); + // Node Parameter node_param_.update_rate = declare_parameter("update_rate"); node_param_.visualize_lanelet = declare_parameter("visualize_lanelet"); @@ -132,8 +139,10 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o node_param_.include_left_lanes = declare_parameter("include_left_lanes"); node_param_.include_opposite_lanes = declare_parameter("include_opposite_lanes"); node_param_.include_conflicting_lanes = declare_parameter("include_conflicting_lanes"); - node_param_.road_border_departure_checker = - declare_parameter("road_border_departure_checker"); + + // Boundary_departure_checker + node_param_.boundary_types_to_detect = + declare_parameter>("boundary_types_to_detect"); // Vehicle Info const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -338,6 +347,7 @@ void LaneDepartureCheckerNode::onTimer() input_.shoulder_lanelets = shoulder_lanelets_; input_.reference_trajectory = reference_trajectory_; input_.predicted_trajectory = predicted_trajectory_; + input_.boundary_types_to_detect = node_param_.boundary_types_to_detect; processing_time_map["Node: setInputData"] = stop_watch.toc(true); output_ = lane_departure_checker_->update(input_); @@ -377,12 +387,19 @@ rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter( result.reason = "success"; try { + // Enable feature + update_param(parameters, "will_out_of_lane_checker", node_param_.will_out_of_lane_checker); + update_param(parameters, "out_of_lane_checker", node_param_.out_of_lane_checker); + update_param(parameters, "boundary_departure_checker", node_param_.boundary_departure_checker); + // Node update_param(parameters, "visualize_lanelet", node_param_.visualize_lanelet); update_param(parameters, "include_right_lanes", node_param_.include_right_lanes); update_param(parameters, "include_left_lanes", node_param_.include_left_lanes); update_param(parameters, "include_opposite_lanes", node_param_.include_opposite_lanes); update_param(parameters, "include_conflicting_lanes", node_param_.include_conflicting_lanes); + update_param(parameters, "boundary_departure_checker", node_param_.boundary_departure_checker); + update_param(parameters, "boundary_types_to_detect", node_param_.boundary_types_to_detect); // Core update_param(parameters, "footprint_margin_scale", param_.footprint_margin_scale); @@ -409,19 +426,19 @@ void LaneDepartureCheckerNode::checkLaneDeparture( int8_t level = DiagStatus::OK; std::string msg = "OK"; - if (output_.will_leave_lane) { + if (output_.will_leave_lane && node_param_.will_out_of_lane_checker) { level = DiagStatus::WARN; msg = "vehicle will leave lane"; } - if (output_.is_out_of_lane) { + if (output_.is_out_of_lane && node_param_.out_of_lane_checker) { level = DiagStatus::ERROR; msg = "vehicle is out of lane"; } - if (output_.will_cross_road_border && node_param_.road_border_departure_checker) { + if (output_.will_cross_boundary && node_param_.boundary_departure_checker) { level = DiagStatus::ERROR; - msg = "vehicle will cross road border"; + msg = "vehicle will cross boundary"; } stat.summary(level, msg); diff --git a/control/operation_mode_transition_manager/README.md b/control/operation_mode_transition_manager/README.md index 3bbc3a006c73a..3cb07a9bd7821 100644 --- a/control/operation_mode_transition_manager/README.md +++ b/control/operation_mode_transition_manager/README.md @@ -25,7 +25,27 @@ There is also an `In Transition` state that occurs during each mode transitions. ## Design - +A rough design of the relationship between `operation_mode_transition_manager`` and the other nodes is shown below. + +![transition_rough_structure](image/transition_rough_structure.drawio.svg) + +A more detailed structure is below. + +![transition_detailed_structure](image/transition_detailed_structure.drawio.svg) + +Here we see that `operation_mode_transition_manager` has multiple state transitions as follows + +- **AUTOWARE ENABLED <---> DISABLED** + - **ENABLED**: the vehicle is controlled by Autoware. + - **DISABLED**: the vehicle is out of Autoware control, expecting the e.g. manual driving. +- **AUTOWARE ENABLED <---> AUTO/LOCAL/REMOTE/NONE** + - **AUTO**: the vehicle is controlled by Autoware, with the autonomous control command calculated by the planning/control component. + - **LOCAL**: the vehicle is controlled by Autoware, with the locally connected operator, e.g. joystick controller. + - **REMOTE**: the vehicle is controlled by Autoware, with the remotely connected operator. + - **NONE**: the vehicle is not controlled by any operator. +- **IN TRANSITION <---> COMPLETED** + - **IN TRANSITION**: the mode listed above is in the transition process, expecting the former operator to have a responsibility to confirm the transition is completed. + - **COMPLETED**: the mode transition is completed. ## Inputs / Outputs / API @@ -63,13 +83,14 @@ For the backward compatibility (to be removed): ## Parameters -| Name | Type | Description | Default value | -| :--------------------------------- | :------- | :------------------------------------------------------------------------------------------------ | :------------ | -| `transition_timeout` | `double` | If the state transition is not completed within this time, it is considered a transition failure. | 10.0 | -| `frequency_hz` | `double` | running hz | 10.0 | -| `check_engage_condition` | `double` | If false, autonomous transition is always available | 0.1 | -| `nearest_dist_deviation_threshold` | `double` | distance threshold used to find nearest trajectory point | 3.0 | -| `nearest_yaw_deviation_threshold` | `double` | angle threshold used to find nearest trajectory point | 1.57 | +| Name | Type | Description | Default value | +| :--------------------------------- | :------- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `transition_timeout` | `double` | If the state transition is not completed within this time, it is considered a transition failure. | 10.0 | +| `frequency_hz` | `double` | running hz | 10.0 | +| `enable_engage_on_driving` | `bool` | Set true if you want to engage the autonomous driving mode while the vehicle is driving. If set to false, it will deny Engage in any situation where the vehicle speed is not zero. Note that if you use this feature without adjusting the parameters, it may cause issues like sudden deceleration. Before using, please ensure the engage condition and the vehicle_cmd_gate transition filter are appropriately adjusted. | 0.1 | +| `check_engage_condition` | `bool` | If false, autonomous transition is always available | 0.1 | +| `nearest_dist_deviation_threshold` | `double` | distance threshold used to find nearest trajectory point | 3.0 | +| `nearest_yaw_deviation_threshold` | `double` | angle threshold used to find nearest trajectory point | 1.57 | For `engage_acceptable_limits` related parameters: @@ -94,6 +115,21 @@ For `stable_check` related parameters: | `speed_upper_threshold` | `double` | the velocity deviation between control command and ego vehicle must be within this threshold to complete `Autonomous` transition. | 2.0 | | `speed_lower_threshold` | `double` | the velocity deviation between control command and ego vehicle must be within this threshold to complete `Autonomous` transition. | 2.0 | +## Engage check behavior on each parameter setting + +This matrix describes the scenarios in which the vehicle can be engaged based on the combinations of parameter settings: + +| `enable_engage_on_driving` | `check_engage_condition` | `allow_autonomous_in_stopped` | Scenarios where engage is permitted | +| :------------------------: | :----------------------: | :---------------------------: | :---------------------------------------------------------------- | +| x | x | x | Only when the vehicle is stationary. | +| x | x | o | Only when the vehicle is stationary. | +| x | o | x | When the vehicle is stationary and all engage conditions are met. | +| x | o | o | Only when the vehicle is stationary. | +| o | x | x | At any time (Caution: Not recommended). | +| o | x | o | At any time (Caution: Not recommended). | +| o | o | x | When all engage conditions are met, regardless of vehicle status. | +| o | o | o | When all engage conditions are met or the vehicle is stationary. | + ## Future extensions / Unimplemented parts - Need to remove backward compatibility interfaces. diff --git a/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml b/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml index a86443f5cabdb..67ce4b485e8c3 100644 --- a/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml +++ b/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml @@ -2,11 +2,16 @@ ros__parameters: transition_timeout: 10.0 frequency_hz: 10.0 + + # set true if you want to engage the autonomous driving mode while the vehicle is driving. If set to false, it will deny Engage in any situation where the vehicle speed is not zero. Note that if you use this feature without adjusting the parameters, it may cause issues like sudden deceleration. Before using, please ensure the engage condition and the vehicle_cmd_gate transition filter are appropriately adjusted. + enable_engage_on_driving: false + check_engage_condition: false # set false if you do not want to care about the engage condition. + nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index engage_acceptable_limits: - allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. + allow_autonomous_in_stopped: true # if true, all engage check is skipped. dist_threshold: 1.5 yaw_threshold: 0.524 speed_upper_threshold: 10.0 diff --git a/control/operation_mode_transition_manager/image/transition_detailed_structure.drawio.svg b/control/operation_mode_transition_manager/image/transition_detailed_structure.drawio.svg new file mode 100644 index 0000000000000..3492dca6dd86b --- /dev/null +++ b/control/operation_mode_transition_manager/image/transition_detailed_structure.drawio.svg @@ -0,0 +1,610 @@ + + + + + + + + + + + + +
+
+
+ Control Command +
+ (Auto) +
+
+
+ + Control Command... + + + + + + +
+
+
+ Trajectory Follower +
+
+
+ + Trajectory Follower + + + + + + + + + +
+
+
+ Control Command +
+
+
+ + Control Command + + + + + + +
+
+
+ Vehicle Cmd Gate +
+
+
+ + Vehicle Cmd Gate + + + + + + +
+
+
+ Vehicle Interface +
+
+
+ + Vehicle Interface + + + + + + + +
+
+
+ Current Mode +
+
+
+ + Current Mode + + + + + + + +
+
+
+ Auto mode +
+ Availability +
+
+
+ + Auto mode... + + + + + + + +
+
+
+ Mode Change +
+ Request +
+
+
+ + Mode Change... + + + + + + +
+
+
+ Operation Mode +
+ Transition Manager +
+
+
+ + Operation Mode... + + + + + + + +
+
+
+ Mode Change +
+ Request +
+
+
+ + Mode Change... + + + + + + +
+
+
+ API +
+
+
+ + API + + + + + + +
+
+
+ Trajectory (From Planning) +
+
+
+ + Trajectory (From Planning) + + + + + + + +
+
+
+ Mode Change +
+ Response +
+
+
+ + Mode Change... + + + + + + + + + + +
+
+
+ Ego pose, twist, acceleration (From Localization) +
+
+
+ + Ego pose, twist, accelera... + + + + + + + +
+
+
+ External Control Command +
+
+
+ + External C... + + + + + + +
+
+
+ AUTOWARE ENABLED +
+
+
+ + AUTOWARE ENABLED + + + + + + +
+
+
+ REMOTE +
+
+
+ + REMOTE + + + + + + + + + +
+
+
+ NONE +
+
+
+ + NONE + + + + + + + + + + + + +
+
+
+ AUTO +
+
+
+ + AUTO + + + + + + + + + + + + +
+
+
+ LOCAL +
+
+
+ + LOCAL + + + + + + + + + +
+
+
+ + AUTOWARE +
+ DISABLED + +
+
+
+ + AUTOWARE... + + + + + + + + + +
+
+
+ TRANSITION +
+
+
+ + TRANSITION + + + + + + +
+
+
+ IN TRANSITION +
+
+
+ + IN TRANSITION + + + + + + +
+
+
+ COMPLETED +
+
+
+ + COMPLETED + + + + + + + + + + + +
+
+
+ + Vehicle Control mode +
+ (From Vehicle) + +
+
+
+ + Vehicle Control mode... + + + + + + +
+
+
+ + Control Component + +
+
+
+ + Control Component + + + + + + +
+
+
+ External Command +
+ Selector +
+
+
+ + External Command... + + + + + + + +
+
+
+ Control Command +
+ (Local) +
+
+
+ + Control Co... + + + + + + + +
+
+
+ Control Command +
+ (Remote) +
+
+
+ + Control Command... + + + + + + + Text is not SVG - cannot display + + + diff --git a/control/operation_mode_transition_manager/image/transition_rough_structure.drawio.svg b/control/operation_mode_transition_manager/image/transition_rough_structure.drawio.svg new file mode 100644 index 0000000000000..073a2315355be --- /dev/null +++ b/control/operation_mode_transition_manager/image/transition_rough_structure.drawio.svg @@ -0,0 +1,321 @@ + + + + + + + + + + + +
+
+
+ Control Command +
+
+
+ + Control Command + + + + + + +
+
+
+ Trajectory Follower +
+
+
+ + Trajectory Follower + + + + + + + +
+
+
+ Control Command +
+
+
+ + Control Command + + + + + + +
+
+
+ Vehicle Cmd Gate +
+
+
+ + Vehicle Cmd Gate + + + + + + +
+
+
+ Vehicle Interface +
+
+
+ + Vehicle Interface + + + + + + + +
+
+
+ Current Mode +
+
+
+ + Current Mode + + + + + + + +
+
+
+ Auto mode +
+ Availability +
+
+
+ + Auto mode... + + + + + + + +
+
+
+ Mode Change +
+ Request +
+
+
+ + Mode Change... + + + + + + +
+
+
+ Operation Mode +
+ Transition Manager +
+
+
+ + Operation Mode... + + + + + + + +
+
+
+ Mode Change +
+ Request +
+
+
+ + Mode Change... + + + + + + +
+
+
+ API +
+
+
+ + API + + + + + + + + +
+
+
+ Change the control mode (e.g. Auto/Manual) for the request, follow the control command in the Auto mode +
+
+
+ + Change the control mode (e.g. Auto/Ma... + + + + + + +
+
+
+ Manage the operation mode transition (e.g. Manual/Auto). +
+ It also rejects the request based on the driving conditrion, checks the transition is complited. +
+
+
+ + Manage the operation mode transition... + + + + + + + + +
+
+
+ Manage the operation mode transition (e.g. Manual/Auto). +
+ It also rejects the request, checks the transition is complited. +
+
+
+ + Manage the operation mode transition... + + + + + + +
+
+
+ Apply filter based on the current operation mode (e.g. Manual/Auto, in the process of the transition) +
+
+
+ + Apply filter based on t... + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index a17e491ebe98d..55b672aa48964 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -46,6 +46,7 @@ AutonomousMode::AutonomousMode(rclcpp::Node * node) "trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = *msg; }); check_engage_condition_ = node->declare_parameter("check_engage_condition"); + enable_engage_on_driving_ = node->declare_parameter("enable_engage_on_driving"); nearest_dist_deviation_threshold_ = node->declare_parameter("nearest_dist_deviation_threshold"); nearest_yaw_deviation_threshold_ = @@ -122,14 +123,24 @@ bool AutonomousMode::isModeChangeCompleted() const auto closest_point = trajectory_.points.at(*closest_idx); // check for lateral deviation - const auto dist_deviation = calcDistance2d(closest_point.pose, kinematics_.pose.pose); + const auto dist_deviation = + motion_utils::calcLateralOffset(trajectory_.points, kinematics_.pose.pose.position); + if (std::isnan(dist_deviation)) { + RCLCPP_INFO(logger_, "Not stable yet: lateral offset calculation failed."); + return unstable(); + } if (dist_deviation > stable_check_param_.dist_threshold) { RCLCPP_INFO(logger_, "Not stable yet: distance deviation is too large: %f", dist_deviation); return unstable(); } // check for yaw deviation - const auto yaw_deviation = calcYawDeviation(closest_point.pose, kinematics_.pose.pose); + const auto yaw_deviation = + motion_utils::calcYawDeviation(trajectory_.points, kinematics_.pose.pose); + if (std::isnan(yaw_deviation)) { + RCLCPP_INFO(logger_, "Not stable yet: lateral offset calculation failed."); + return unstable(); + } if (yaw_deviation > stable_check_param_.yaw_threshold) { RCLCPP_INFO(logger_, "Not stable yet: yaw deviation is too large: %f", yaw_deviation); return unstable(); @@ -208,6 +219,15 @@ bool AutonomousMode::isModeChangeAvailable() const auto target_control_speed = control_cmd_.longitudinal.speed; const auto & param = engage_acceptable_param_; + if (!enable_engage_on_driving_ && std::fabs(current_speed) > 1.0e-2) { + RCLCPP_INFO( + logger_, + "Engage unavailable: enable_engage_on_driving is false, and the vehicle is not " + "stationary."); + debug_info_ = DebugInfo{}; // all false + return false; + } + if (trajectory_.points.size() < 2) { RCLCPP_WARN_SKIPFIRST_THROTTLE( logger_, *clock_, 5000, "Engage unavailable: trajectory size must be > 2"); diff --git a/control/operation_mode_transition_manager/src/state.hpp b/control/operation_mode_transition_manager/src/state.hpp index 9d384857bbe3d..de0dd8a543b15 100644 --- a/control/operation_mode_transition_manager/src/state.hpp +++ b/control/operation_mode_transition_manager/src/state.hpp @@ -73,6 +73,7 @@ class AutonomousMode : public ModeChangeBase rclcpp::Clock::SharedPtr clock_; bool check_engage_condition_ = true; // if false, the vehicle is engaged without any checks. + bool enable_engage_on_driving_ = false; // if false, engage is not permited on driving double nearest_dist_deviation_threshold_; // [m] for finding nearest index double nearest_yaw_deviation_threshold_; // [rad] for finding nearest index EngageAcceptableParam engage_acceptable_param_; diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp index 8dec06c455868..6a33855fb4d0a 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -104,25 +104,28 @@ void VehicleCmdFilter::VehicleCmdFilter::limitLongitudinalWithJerk( std::clamp(static_cast(input.longitudinal.jerk), -lon_jerk_lim, lon_jerk_lim); } +// Use ego vehicle speed (not speed command) for the lateral acceleration calculation, otherwise the +// filtered steering angle oscillates if the input velocity oscillates. void VehicleCmdFilter::limitLateralWithLatAcc( [[maybe_unused]] const double dt, AckermannControlCommand & input) const { const auto lat_acc_lim = getLatAccLim(); - double latacc = calcLatAcc(input); + double latacc = calcLatAcc(input, current_speed_); if (std::fabs(latacc) > lat_acc_lim) { - double v_sq = - std::max(static_cast(input.longitudinal.speed * input.longitudinal.speed), 0.001); + double v_sq = std::max(static_cast(current_speed_ * current_speed_), 0.001); double steer_lim = std::atan(lat_acc_lim * param_.wheel_base / v_sq); input.lateral.steering_tire_angle = latacc > 0.0 ? steer_lim : -steer_lim; } } +// Use ego vehicle speed (not speed command) for the lateral acceleration calculation, otherwise the +// filtered steering angle oscillates if the input velocity oscillates. void VehicleCmdFilter::limitLateralWithLatJerk( const double dt, AckermannControlCommand & input) const { - double curr_latacc = calcLatAcc(input); - double prev_latacc = calcLatAcc(prev_cmd_); + double curr_latacc = calcLatAcc(input, current_speed_); + double prev_latacc = calcLatAcc(prev_cmd_, current_speed_); const auto lat_jerk_lim = getLatJerkLim(); @@ -130,9 +133,9 @@ void VehicleCmdFilter::limitLateralWithLatJerk( const double latacc_min = prev_latacc - lat_jerk_lim * dt; if (curr_latacc > latacc_max) { - input.lateral.steering_tire_angle = calcSteerFromLatacc(input.longitudinal.speed, latacc_max); + input.lateral.steering_tire_angle = calcSteerFromLatacc(current_speed_, latacc_max); } else if (curr_latacc < latacc_min) { - input.lateral.steering_tire_angle = calcSteerFromLatacc(input.longitudinal.speed, latacc_min); + input.lateral.steering_tire_angle = calcSteerFromLatacc(current_speed_, latacc_min); } } @@ -205,6 +208,11 @@ double VehicleCmdFilter::calcLatAcc(const AckermannControlCommand & cmd) const return v * v * std::tan(cmd.lateral.steering_tire_angle) / param_.wheel_base; } +double VehicleCmdFilter::calcLatAcc(const AckermannControlCommand & cmd, const double v) const +{ + return v * v * std::tan(cmd.lateral.steering_tire_angle) / param_.wheel_base; +} + double VehicleCmdFilter::limitDiff( const double curr, const double prev, const double diff_lim) const { diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp index eb85fcbeb8606..a79b8a38bae8d 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp @@ -81,6 +81,7 @@ class VehicleCmdFilter bool setParameterWithValidation(const VehicleCmdFilterParam & p); double calcLatAcc(const AckermannControlCommand & cmd) const; + double calcLatAcc(const AckermannControlCommand & cmd, const double v) const; double calcSteerFromLatacc(const double v, const double latacc) const; double limitDiff(const double curr, const double prev, const double diff_lim) const; diff --git a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp index 6384fbfb22f60..1156656385117 100644 --- a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp +++ b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp @@ -150,7 +150,6 @@ class PubSubNode : public rclcpp::Node if (!cmd_history_.empty()) { // ego moves as commanded. msg.twist.twist.linear.x = cmd_history_.back()->longitudinal.speed; // ego moves as commanded. - } else { } pub_odom_->publish(msg); } @@ -238,16 +237,19 @@ class PubSubNode : public rclcpp::Node const auto lon_jerk = (lon_acc - cmd_prev->longitudinal.acceleration) / dt; const auto lat_acc = lon_vel * lon_vel * std::tan(cmd_curr->lateral.steering_tire_angle) / wheelbase; - const auto prev_lon_vel = cmd_prev->longitudinal.speed; + + // TODO(Horibe): prev_lat_acc should use the previous velocity, but use the current velocity + // since the current filtering logic uses the current velocity. const auto prev_lat_acc = - prev_lon_vel * prev_lon_vel * std::tan(cmd_prev->lateral.steering_tire_angle) / wheelbase; + lon_vel * lon_vel * std::tan(cmd_prev->lateral.steering_tire_angle) / wheelbase; const auto lat_jerk = (lat_acc - prev_lat_acc) / dt; /* debug print */ // const auto steer = cmd_curr->lateral.steering_tire_angle; // PRINT_VALUES( - // dt, lon_vel, lon_acc, lon_jerk, lat_acc, lat_jerk, steer, max_lon_acc_lim, - // max_lon_jerk_lim, max_lat_acc_lim, max_lat_jerk_lim); + // dt, i_curr, i_prev, steer, lon_vel, prev_lon_vel, lon_acc, lon_jerk, lat_acc, prev_lat_acc, + // prev_lat_acc2, lat_jerk, max_lon_acc_lim, max_lon_jerk_lim, max_lat_acc_lim, + // max_lat_jerk_lim); // Output command must be smaller than maximum limit. // TODO(Horibe): check for each velocity range. @@ -368,6 +370,16 @@ TEST_P(TestFixture, CheckFilterForSinCmd) [[maybe_unused]] auto b = std::system("ros2 node info /test_vehicle_cmd_gate_filter_pubsub"); [[maybe_unused]] auto c = std::system("ros2 node info /vehicle_cmd_gate"); + // std::cerr << "speed signal: " << cmd_generator_.p_.velocity.max << " * sin(2pi * " + // << cmd_generator_.p_.velocity.freq << " * dt + " << cmd_generator_.p_.velocity.bias + // << ")" << std::endl; + // std::cerr << "accel signal: " << cmd_generator_.p_.acceleration.max << " * sin(2pi * " + // << cmd_generator_.p_.acceleration.freq << " * dt + " + // << cmd_generator_.p_.acceleration.bias << ")" << std::endl; + // std::cerr << "steer signal: " << cmd_generator_.p_.steering.max << " * sin(2pi * " + // << cmd_generator_.p_.steering.freq << " * dt + " << cmd_generator_.p_.steering.bias + // << ")" << std::endl; + for (size_t i = 0; i < 100; ++i) { const bool reset_clock = (i == 0); const auto cmd = cmd_generator_.calcSinWaveCommand(reset_clock); @@ -384,17 +396,23 @@ TEST_P(TestFixture, CheckFilterForSinCmd) }; // High frequency, large value -CmdParams p1 = {/*steer*/ {10, 1, 0}, /*velocity*/ {10, 1.2, 0}, /*acc*/ {5, 1.5, 2}}; +CmdParams p1 = {/*steer*/ {0.5, 1, 0}, /*velocity*/ {10, 0.0, 0}, /*acc*/ {5, 1.5, 2}}; INSTANTIATE_TEST_SUITE_P(TestParam1, TestFixture, ::testing::Values(p1)); // High frequency, normal value -CmdParams p2 = {/*steer*/ {1.5, 2, 1}, /*velocity*/ {5, 1.0, 0}, /*acc*/ {2.0, 3.0, 2}}; +CmdParams p2 = {/*steer*/ {0.5, 2, 1}, /*velocity*/ {5, 1.0, 0}, /*acc*/ {2.0, 3.0, 2}}; INSTANTIATE_TEST_SUITE_P(TestParam2, TestFixture, ::testing::Values(p2)); // High frequency, small value -CmdParams p3 = {/*steer*/ {1.5, 3, 2}, /*velocity*/ {2, 3, 0}, /*acc*/ {0.5, 3, 2}}; +CmdParams p3 = {/*steer*/ {0.5, 3, 2}, /*velocity*/ {2, 3, 0}, /*acc*/ {0.5, 3, 2}}; INSTANTIATE_TEST_SUITE_P(TestParam3, TestFixture, ::testing::Values(p3)); // Low frequency -CmdParams p4 = {/*steer*/ {10, 0.1, 0.5}, /*velocity*/ {10, 0.2, 0}, /*acc*/ {5, 0.1, 2}}; +CmdParams p4 = {/*steer*/ {0.5, 0.1, 0.5}, /*velocity*/ {10, 0.2, 0}, /*acc*/ {5, 0.1, 2}}; INSTANTIATE_TEST_SUITE_P(TestParam4, TestFixture, ::testing::Values(p4)); + +// Large steer, large velocity -> this test fails. +// Lateral acceleration and lateral jerk affect both steer and velocity, and if both steer and +// velocity changes significantly, the current logic cannot adequately handle the situation. +// CmdParams p5 = {/*steer*/ {10.0, 1.0, 0.5}, /*velocity*/ {10, 0.2, 0}, /*acc*/ {5, 0.1, 2}}; +// INSTANTIATE_TEST_SUITE_P(TestParam5, TestFixture, ::testing::Values(p5)); diff --git a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp index 7ce8580120652..1791493aeb17b 100644 --- a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp @@ -56,12 +56,20 @@ AckermannControlCommand genCmd(double s, double sr, double v, double a) return cmd; } +// calc from ego velocity +double calcLatAcc(const AckermannControlCommand & cmd, const double wheelbase, const double ego_v) +{ + return ego_v * ego_v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; +} + +// calc from command velocity double calcLatAcc(const AckermannControlCommand & cmd, const double wheelbase) { double v = cmd.longitudinal.speed; return v * v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; } +// calc from command velocity double calcLatJerk( const AckermannControlCommand & cmd, const AckermannControlCommand & prev_cmd, const double wheelbase, const double dt) @@ -75,14 +83,28 @@ double calcLatJerk( return (curr - prev) / dt; } +// calc from ego velocity +double calcLatJerk( + const AckermannControlCommand & cmd, const AckermannControlCommand & prev_cmd, + const double wheelbase, const double dt, const double ego_v) +{ + const auto prev = ego_v * ego_v * std::tan(prev_cmd.lateral.steering_tire_angle) / wheelbase; + + const auto curr = ego_v * ego_v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; + + return (curr - prev) / dt; +} + void test_1d_limit( - double V_LIM, double A_LIM, double J_LIM, double LAT_A_LIM, double LAT_J_LIM, double STEER_DIFF, - const AckermannControlCommand & prev_cmd, const AckermannControlCommand & raw_cmd) + double ego_v, double V_LIM, double A_LIM, double J_LIM, double LAT_A_LIM, double LAT_J_LIM, + double STEER_DIFF, const AckermannControlCommand & prev_cmd, + const AckermannControlCommand & raw_cmd) { const double WHEELBASE = 3.0; const double DT = 0.1; // [s] vehicle_cmd_gate::VehicleCmdFilter filter; + filter.setCurrentSpeed(ego_v); setFilterParams( filter, V_LIM, {0.0}, {A_LIM}, {J_LIM}, {LAT_A_LIM}, {LAT_J_LIM}, {STEER_DIFF}, WHEELBASE); filter.setPrevCmd(prev_cmd); @@ -153,8 +175,8 @@ void test_1d_limit( { auto filtered_cmd = raw_cmd; filter.limitLateralWithLatAcc(DT, filtered_cmd); - const double filtered_latacc = calcLatAcc(filtered_cmd, WHEELBASE); - const double raw_latacc = calcLatAcc(raw_cmd, WHEELBASE); + const double filtered_latacc = calcLatAcc(filtered_cmd, WHEELBASE, ego_v); + const double raw_latacc = calcLatAcc(raw_cmd, WHEELBASE, ego_v); // check if the filtered value does not exceed the limit. ASSERT_LT_NEAR(std::abs(filtered_latacc), LAT_A_LIM); @@ -169,9 +191,9 @@ void test_1d_limit( { auto filtered_cmd = raw_cmd; filter.limitLateralWithLatJerk(DT, filtered_cmd); - const double prev_lat_acc = calcLatAcc(prev_cmd, WHEELBASE); - const double filtered_lat_acc = calcLatAcc(filtered_cmd, WHEELBASE); - const double raw_lat_acc = calcLatAcc(raw_cmd, WHEELBASE); + const double prev_lat_acc = calcLatAcc(prev_cmd, WHEELBASE, ego_v); + const double filtered_lat_acc = calcLatAcc(filtered_cmd, WHEELBASE, ego_v); + const double raw_lat_acc = calcLatAcc(raw_cmd, WHEELBASE, ego_v); const double filtered_lateral_jerk = (filtered_lat_acc - prev_lat_acc) / DT; // check if the filtered value does not exceed the limit. @@ -211,6 +233,7 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) const std::vector lat_a_arr = {0.01, 1.0, 100.0}; const std::vector lat_j_arr = {0.01, 1.0, 100.0}; const std::vector steer_diff_arr = {0.01, 1.0, 100.0}; + const std::vector ego_v_arr = {0.0, 0.1, 1.0, 3.0, 15.0}; const std::vector prev_cmd_arr = { genCmd(0.0, 0.0, 0.0, 0.0), genCmd(1.0, 1.0, 1.0, 1.0)}; @@ -226,7 +249,9 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) for (const auto & prev_cmd : prev_cmd_arr) { for (const auto & raw_cmd : raw_cmd_arr) { for (const auto & steer_diff : steer_diff_arr) { - test_1d_limit(v, a, j, la, lj, steer_diff, prev_cmd, raw_cmd); + for (const auto & ego_v : ego_v_arr) { + test_1d_limit(ego_v, v, a, j, la, lj, steer_diff, prev_cmd, raw_cmd); + } } } } @@ -371,66 +396,72 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) // lateral acc lim // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; // p.lat_acc_lim = std::vector{0.1, 0.2, 0.3}; - const auto _calcLatAcc = [&](const auto & cmd) { return calcLatAcc(cmd, WHEELBASE); }; + const auto _calcLatAcc = [&](const auto & cmd, const double ego_v) { + return calcLatAcc(cmd, WHEELBASE, ego_v); + }; { + // since the lateral acceleration is 0 when the velocity is 0, the target value is 0 only in + // this case set_speed_and_reset_prev(0.0); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.1, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 0.0), 0.0, ep); set_speed_and_reset_prev(2.0); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.1, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 2.0), 0.1, ep); set_speed_and_reset_prev(3.0); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.15, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 3.0), 0.15, ep); set_speed_and_reset_prev(5.0); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.2 + 0.1 / 6.0, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 5.0), 0.2 + 0.1 / 6.0, ep); set_speed_and_reset_prev(8.0); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.2 + 0.4 / 6.0, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 8.0), 0.2 + 0.4 / 6.0, ep); set_speed_and_reset_prev(10.0); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.3, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 10.0), 0.3, ep); set_speed_and_reset_prev(15.0); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd)), 0.3, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatAcc(orig_cmd), 15.0), 0.3, ep); } // lateral jerk lim // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; // p.lat_jerk_lim = std::vector{0.9, 0.7, 0.1}; - const auto _calcLatJerk = [&](const auto & cmd) { - return calcLatJerk(cmd, AckermannControlCommand{}, WHEELBASE, DT); + const auto _calcLatJerk = [&](const auto & cmd, const double ego_v) { + return calcLatJerk(cmd, AckermannControlCommand{}, WHEELBASE, DT, ego_v); }; { + // since the lateral acceleration and jerk is 0 when the velocity is 0, the target value is 0 + // only in this case set_speed_and_reset_prev(0.0); - EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.9, ep); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.9, ep); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 0.0), 0.0, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 0.0), DT * 0.0, ep); set_speed_and_reset_prev(2.0); - EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.9, ep); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.9, ep); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 2.0), 0.9, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 2.0), DT * 0.9, ep); set_speed_and_reset_prev(3.0); - EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.8, ep); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.8, ep); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 3.0), 0.8, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 3.0), DT * 0.8, ep); set_speed_and_reset_prev(5.0); const auto expect_v5 = 0.7 - 0.6 * (1.0 / 6.0); - EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), expect_v5, ep); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * expect_v5, ep); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 5.0), expect_v5, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 5.0), DT * expect_v5, ep); set_speed_and_reset_prev(8.0); const auto expect_v8 = 0.7 - 0.6 * (4.0 / 6.0); - EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), expect_v8, ep); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * expect_v8, ep); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 8.0), expect_v8, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 8.0), DT * expect_v8, ep); set_speed_and_reset_prev(10.0); - EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.1, ep); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.1, ep); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 10.0), 0.1, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 10.0), DT * 0.1, ep); set_speed_and_reset_prev(15.0); - EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd)), 0.1, ep); - EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd)), DT * 0.1, ep); + EXPECT_NEAR(_calcLatJerk(_limitLateralWithLatJerk(orig_cmd), 15.0), 0.1, ep); + EXPECT_NEAR(_calcLatAcc(_limitLateralWithLatJerk(orig_cmd), 15.0), DT * 0.1, ep); } // steering diff lim diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml index 4de9aec81bb38..642edb4f68ab8 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml @@ -4,8 +4,9 @@ - - + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index c74c0fdcc63b7..eab2a56607dee 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -79,12 +79,54 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -96,6 +138,8 @@ + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 99f09cfe6abbd..463340efdecfe 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -143,12 +143,54 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml index a9301f31d87aa..58e9e231e4aa0 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml @@ -15,6 +15,7 @@ + @@ -143,6 +144,7 @@ + diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index c1723c1fa07e8..336588891d9b2 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -12,6 +12,7 @@ ament_cmake_auto autoware_cmake + cluster_merger compare_map_segmentation crosswalk_traffic_light_estimator detected_object_feature_remover 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/README.md b/localization/ar_tag_based_localizer/README.md index e471378773cbb..81bc11a7cbc9b 100644 --- a/localization/ar_tag_based_localizer/README.md +++ b/localization/ar_tag_based_localizer/README.md @@ -18,10 +18,11 @@ This package includes two nodes. #### Input -| Name | Type | Description | -| :-------------------- | :----------------------------- | :----------- | -| `~/input/image` | `sensor_msgs::msg::Image` | Camera Image | -| `~/input/camera_info` | `sensor_msgs::msg::CameraInfo` | Camera Info | +| Name | Type | Description | +| :-------------------- | :---------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `~/input/image` | `sensor_msgs::msg::Image` | Camera Image | +| `~/input/camera_info` | `sensor_msgs::msg::CameraInfo` | Camera Info | +| `~/input/ekf_pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose without IMU correction. It is used to validate detected AR tags by filtering out False Positives. Only if the EKF Pose and the AR tag-detected Pose are within a certain temporal and spatial range, the AR tag-detected Pose is considered valid and published. | #### Output @@ -55,7 +56,26 @@ ros2 launch autoware_launch ... \ ... ``` -[Sample rosbag and map](https://drive.google.com/file/d/1wiCQjyjRnYbb0dg8G6mRecdSGh8tv3zR/view) +### Rosbag + +#### [Sample rosbag and map (AWSIM data)](https://drive.google.com/file/d/1uMVwQQFcfs8JOqfoA1FqfH_fLPwQ71jK/view) + +This data is simulated data created by [AWSIM](https://tier4.github.io/AWSIM/). +Essentially, AR tag-based self-localization is not intended for such public road driving, but for driving in a smaller area, so the max driving speed is set at 15 km/h. + +It is a known problem that the timing of when each AR tag begins to be detected can cause significant changes in estimation. + +![sample_result_in_awsim](./doc_image/sample_result_in_awsim.png) + +#### [Sample rosbag and map (Real world data)](https://drive.google.com/file/d/1wiCQjyjRnYbb0dg8G6mRecdSGh8tv3zR/view) + +Please remap the topic names and play it. + +```bash +ros2 bag play /path/to/ar_tag_based_localizer_sample_bag/ -r 0.5 -s sqlite3 \ + --remap /sensing/camera/front/image:=/sensing/camera/traffic_light/image_raw \ + /sensing/camera/front/image/info:=/sensing/camera/traffic_light/camera_info +``` This dataset contains issues such as missing IMU data, and overall the accuracy is low. Even when running AR tag-based self-localization, significant difference from the true trajectory can be observed. diff --git a/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml b/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml index 385e86498c58c..29260e27cd43c 100644 --- a/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml +++ b/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml @@ -4,23 +4,34 @@ marker_size: 0.6 # target_tag_ids - target_tag_ids: ['0','4','5'] + target_tag_ids: ['0','1','2','3','4','5','6'] - # covariance - covariance: [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.01] + # base_covariance + # This value is dynamically scaled according to the distance at which AR tags are detected. + base_covariance: [0.2, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.02] # Detect AR-Tags within this range and publish the pose of ego vehicle - distance_threshold: 6.0 # [m] - - # Camera frame id - camera_frame: "camera" + distance_threshold: 13.0 # [m] # Detector parameters # See https://github.com/pal-robotics/aruco_ros/blob/7787a6794d30c248bc546d1582e65dd47bc40c12/aruco/include/aruco/markerdetector.h#L106-L126 detection_mode: "DM_NORMAL" # select from [DM_NORMAL, DM_FAST, DM_VIDEO_FAST] min_marker_size: 0.02 + + # Parameters for comparison with EKF Pose + # If the difference between the EKF pose and the current pose is within the range of values set below, the current pose is published. + # [How to determine the value] + # * ekf_time_tolerance: Since it is abnormal if the data comes too old from EKF, the tentative tolerance value is set at 5 seconds. + # This value is assumed to be unaffected even if it is increased or decreased by some amount. + # * ekf_position_tolerance: Since it is possible that multiple AR tags with the same ID could be placed, the tolerance should be as small as possible. + # And if the vehicle is running only on odometry in a section without AR tags, + # it is possible that self-position estimation could be off by a few meters. + # it should be fixed by AR tag detection, so tolerance should not be smaller than 10 meters. + # Therefore, the tolerance is set at 10 meters. + ekf_time_tolerance: 5.0 # [s] + ekf_position_tolerance: 10.0 # [m] diff --git a/localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg b/localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg index a339951663b29..e41ca0ad019ce 100644 --- a/localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg +++ b/localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg @@ -6,7 +6,7 @@ width="721px" height="331px" viewBox="-0.5 -0.5 721 331" - content="<mxfile><diagram id="z_GJx55S16-dNHi1224P" name="ページ1">1Zhdk5sgFIZ/TS6bEdF8XDYm2V5spzvdnWl75RBFZdaIg+Srv74QMcZAdt3EbKZeJHIAhec9nAP2oLfcPjCUJ99piNOebYXbHpz2bBtYliv+pGVXWhwwKA0xI6FqVBueyV9c9VTWFQlx0WjIKU05yZvGgGYZDnjDhhijm2aziKbNt+YoxprhOUCpbv1FQp6U1pE9rO3fMImT6s1gMC5rlqhqrGZSJCikmyMTnPWgxyjl5d1y6+FUwqu4lP3mZ2oPA2M442062GWHNUpXam5qXHxXTZbRVRZi2d7qwUlEM64EAbYqezSlbN8YWvtL2PWBqLGtMeN4e2RSA3vAdIk524kmqtZ2FSTlJVAVNzXyg0ckR7gHyoaUyvHhyTUIcaNYmLk4GpevP0X5BcXid4IKwcMWPguBnOsjFa4hmLD32JE0PWLleTN3LsYzKTijr/ioZhCM8CK6LV04aNJ1dLpjA1ynA7iuBvcHTyQ96+uK042Y1RHcHAWvYj0VH2U7n3ueie3IXsDB4Maeez+2cGhY0XO+IQX3N4QnfkDXiBGUBVgjikMR5VSRMp7QmGYondXWiUDCdr8V8n3hjyz0h25Vnm6Pa6c7VToLlSMWY2WCyibH8SZmhlPEyboZjU3UVNcnSsRrz7o+dEbNRxR0xQKsep2wPwyjlRxVnG/KQZYywbyFP6PZW7ztTniD4efwdqDb4G0PTxy9HJTGW3uQbTWFsyG8mXAjk3A5LfCHllGl45bwvYx9V5WkjkDd1xrKwu5Y0E61L+k0tb9o/Sk5vlh9IK6y07U+Mmz6CIQf9ZGqIY2iAl8rPzCGUcR8jmJ/IfO/n16Z9c/lH303sL+6yUzO6Qqy9dQEXENucrvITcAEFb9Gl7NUWf4uLAen+1PwmSyN8SlAS8yQT7KIXpNeuknnn5VetKzgtkvnhjzVfBAYua1i0AXxxYEG+d4XrFVoh2NdC6fzrVVrRx1rM31EGU4x12eMirw8rkdkK9f8JMeMiDdiuXrFY8X5Hj/VpomIGXEmqgKBY284HKelk4aoSA6RoxDnCJLFLzQXBigM5R7s8D8lS3G0m6dkIeN8IBH4IWFiOFTOdx4ijmTcL/rFOu4mfoxOvM2Wu4PTAGKKH874elkc/RAmzwmRX3Chf9DGGQ98QVvPrA44t40SrREY9+YywQsMASr4/3WgP03tAIwN/nSzhOTon0w6i2iOIbtU6t3Bb4zbmFQFNX+JcrGfQeHFm5n7fLI49R5jNOrKe0Sx/sZZps/6SzGc/QM=</diagram></mxfile>" + content="<mxfile><diagram id="z_GJx55S16-dNHi1224P" name="ページ1">1ZlLc5swEIB/jY/1WAiwfaydOD2k00yTmbYnRgYBmmDECPnVX1/JCDBIjomNkykHBy0SSN+u9qEM4Hy1e2Aoi7/TACcDaxTsBvBuYFnTiSt+pWBfCGygBBEjQSECteCZ/MVKOFLSNQlw3ujIKU04yZpCn6Yp9nlDhhij22a3kCbNr2Yowprg2UeJLv1FAh4X0ok1ruXfMIni8svAnRZPVqjsrFaSxyig2yMRvB/AOaOUF3er3Rwnkl3JpRi3OPG0mhjDKe8ywCoGbFCyVmtT8+L7crGMrtMAy/6jAZyFNOVKIcBS7TlNKDt0hqPDJeT6RNTcNphxvDsSqYk9YLrCnO1FF/XUchQkZSVQNbc18soi4iPcrpIhpeWoenMNQtwoFmYutsbl60/RfkGR+J2hXPCwhM1CINf6SIVpCCbsHDuSJEes5vN7ZyHmM8s5o6/46InrT/AyvC1d6Dbp2jrdqQGu3QNcR4P7g8eS3ujrmtOtWNUR3Az5r2I/5e9lu1jM5ya2E2sJXffGlvt5bOHYsKMXfEty7m0Jjz2fbhAjKPWxRhQHwsupJmU8phFNUXJfS2cCCdv/VsgPjT+yMRw7Zftud/z0bq9aJ6FyxCKsRFDJ5DzexMxwgjjZNL2xiZoa+kSJ+OxJ04f2pPmKnK6Zj9WoFvtqGp3UUfr5pjrISgaYt/CnNH2Lt9ULbzD+GN42dBq8rXHL0ItJaby1F1mjpuIsCG+muIlJcRnN8bu2UanHHeEHNQ4d1ZJ6BOq+1qFs7I8V2qvuCzpN3V+0/5Q6voyGQFzFoGttZNy0EQjfayNlRxqGOb5W/cDoRhHzOIq8pYz/XnJl1D8Vf/Rs4HD1E5ns9g6y9NAEHENscvqITcAEFb+Gl7NUUf5TWLrt/BR8JEujf/LRCjPkkTSk14SXfsL5R4UXLSo43cK5IU41XwQmTicfdIF/saFBfecV1sm1w6muC7v31KqzoU61lT6iFCeY6ytGeVaU6yHZyT0/yzAj4otY7l7xWlHf46daNBM+I0rFI1/gOAiqcloaaYDyuPIcuagjSBq90EwIoBAUOVj1946sRGm3SMhS+nlfIvACwsR0qFzvIkAcSb+fD/NN1I//mLSszZLZQduBmPyHPb1eLbZehMk6IfRyLvTvdzHGii/oapllgXNbL9EZgTE3lwFeYPBRzv+vgr4d2gGYGuzpZgHJ1o9MevNotiG6lNr7BLsxpjGJcmreCmUin0HBxcnM5xxZtK3H6I1uZz2uialMDd9dcp05uQDNVOdcpmOu3KpizVi5GQ3zfNg+mUL1fwAybmWul2ZMbvskxRkP+8qZRLM+By+61/9MgPf/AA==</diagram></mxfile>" > @@ -82,5 +82,11 @@ /lanelet2_map_loader + + + + + /ekf_pose_with_covariance + diff --git a/localization/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png b/localization/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png new file mode 100644 index 0000000000000..875005214bf5e Binary files /dev/null and b/localization/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png differ diff --git a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp b/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp index 39373d92add25..65c1283a82728 100644 --- a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp +++ b/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp @@ -69,14 +69,17 @@ class ArTagBasedLocalizer : public rclcpp::Node private: void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & msg); void cam_info_callback(const sensor_msgs::msg::CameraInfo & msg); - void publish_pose_as_base_link(const geometry_msgs::msg::PoseStamped & msg); + void ekf_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped & msg); + void publish_pose_as_base_link( + const geometry_msgs::msg::PoseStamped & msg, const std::string & camera_frame_id); // Parameters float marker_size_{}; std::vector target_tag_ids_; - std::vector covariance_; + std::vector base_covariance_; double distance_threshold_squared_{}; - std::string camera_frame_; + double ekf_time_tolerance_{}; + double ekf_position_tolerance_{}; // tf std::unique_ptr tf_buffer_; @@ -87,8 +90,9 @@ class ArTagBasedLocalizer : public rclcpp::Node std::unique_ptr it_; // Subscribers - image_transport::Subscriber image_sub_; + rclcpp::Subscription::SharedPtr image_sub_; rclcpp::Subscription::SharedPtr cam_info_sub_; + rclcpp::Subscription::SharedPtr ekf_pose_sub_; // Publishers image_transport::Publisher image_pub_; @@ -98,6 +102,7 @@ class ArTagBasedLocalizer : public rclcpp::Node aruco::MarkerDetector detector_; aruco::CameraParameters cam_param_; bool cam_info_received_; + geometry_msgs::msg::PoseWithCovarianceStamped latest_ekf_pose_{}; }; #endif // AR_TAG_BASED_LOCALIZER__AR_TAG_BASED_LOCALIZER_CORE_HPP_ diff --git a/localization/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml b/localization/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml index 940f6ed53d8b8..f6c10050b1826 100644 --- a/localization/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml +++ b/localization/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml @@ -5,6 +5,7 @@ + @@ -12,6 +13,7 @@ + diff --git a/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp b/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp index ca3bfe2e4a6b0..cf0236c1a1e48 100644 --- a/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp +++ b/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp @@ -69,9 +69,10 @@ bool ArTagBasedLocalizer::setup() */ marker_size_ = static_cast(this->declare_parameter("marker_size")); target_tag_ids_ = this->declare_parameter>("target_tag_ids"); - covariance_ = this->declare_parameter>("covariance"); + base_covariance_ = this->declare_parameter>("base_covariance"); distance_threshold_squared_ = std::pow(this->declare_parameter("distance_threshold"), 2); - camera_frame_ = this->declare_parameter("camera_frame"); + ekf_time_tolerance_ = this->declare_parameter("ekf_time_tolerance"); + ekf_position_tolerance_ = this->declare_parameter("ekf_position_tolerance"); std::string detection_mode = this->declare_parameter("detection_mode"); float min_marker_size = static_cast(this->declare_parameter("min_marker_size")); if (detection_mode == "DM_NORMAL") { @@ -109,20 +110,25 @@ bool ArTagBasedLocalizer::setup() /* Subscribers */ - image_sub_ = it_->subscribe("~/input/image", 1, &ArTagBasedLocalizer::image_callback, this); + rclcpp::QoS qos_sub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + qos_sub.best_effort(); + image_sub_ = this->create_subscription( + "~/input/image", qos_sub, + std::bind(&ArTagBasedLocalizer::image_callback, this, std::placeholders::_1)); cam_info_sub_ = this->create_subscription( - "~/input/camera_info", 1, + "~/input/camera_info", qos_sub, std::bind(&ArTagBasedLocalizer::cam_info_callback, this, std::placeholders::_1)); + ekf_pose_sub_ = this->create_subscription( + "~/input/ekf_pose", qos_sub, + std::bind(&ArTagBasedLocalizer::ekf_pose_callback, this, std::placeholders::_1)); /* Publishers */ - auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); - qos.reliable(); - qos.transient_local(); + rclcpp::QoS qos_pub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); image_pub_ = it_->advertise("~/debug/result", 1); pose_pub_ = this->create_publisher( - "~/output/pose_with_covariance", qos); + "~/output/pose_with_covariance", qos_pub); RCLCPP_INFO(this->get_logger(), "Setup of ar_tag_based_localizer node is successful!"); return true; @@ -164,7 +170,7 @@ void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSha geometry_msgs::msg::TransformStamped tf_cam_to_marker_stamped; tf2::toMsg(tf_cam_to_marker, tf_cam_to_marker_stamped.transform); tf_cam_to_marker_stamped.header.stamp = curr_stamp; - tf_cam_to_marker_stamped.header.frame_id = camera_frame_; + tf_cam_to_marker_stamped.header.frame_id = msg->header.frame_id; tf_cam_to_marker_stamped.child_frame_id = "detected_marker_" + std::to_string(marker.id); tf_broadcaster_->sendTransform(tf_cam_to_marker_stamped); @@ -172,7 +178,7 @@ void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSha tf2::toMsg(tf_cam_to_marker, pose_cam_to_marker.pose); pose_cam_to_marker.header.stamp = curr_stamp; pose_cam_to_marker.header.frame_id = std::to_string(marker.id); - publish_pose_as_base_link(pose_cam_to_marker); + publish_pose_as_base_link(pose_cam_to_marker, msg->header.frame_id); // drawing the detected markers marker.draw(in_image, cv::Scalar(0, 0, 255), 2); @@ -206,7 +212,14 @@ void ArTagBasedLocalizer::cam_info_callback(const sensor_msgs::msg::CameraInfo & cam_info_received_ = true; } -void ArTagBasedLocalizer::publish_pose_as_base_link(const geometry_msgs::msg::PoseStamped & msg) +void ArTagBasedLocalizer::ekf_pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped & msg) +{ + latest_ekf_pose_ = msg; +} + +void ArTagBasedLocalizer::publish_pose_as_base_link( + const geometry_msgs::msg::PoseStamped & msg, const std::string & camera_frame_id) { // Check if frame_id is in target_tag_ids if ( @@ -241,7 +254,7 @@ void ArTagBasedLocalizer::publish_pose_as_base_link(const geometry_msgs::msg::Po geometry_msgs::msg::TransformStamped camera_to_base_link_tf; try { camera_to_base_link_tf = - tf_buffer_->lookupTransform(camera_frame_, "base_link", tf2::TimePointZero); + tf_buffer_->lookupTransform(camera_frame_id, "base_link", tf2::TimePointZero); } catch (tf2::TransformException & ex) { RCLCPP_INFO(this->get_logger(), "Could not transform base_link to camera: %s", ex.what()); return; @@ -265,8 +278,49 @@ void ArTagBasedLocalizer::publish_pose_as_base_link(const geometry_msgs::msg::Po pose_with_covariance_stamped.header.stamp = msg.header.stamp; pose_with_covariance_stamped.header.frame_id = "map"; pose_with_covariance_stamped.pose.pose = tf2::toMsg(map_to_base_link); - std::copy( - covariance_.begin(), covariance_.end(), pose_with_covariance_stamped.pose.covariance.begin()); + + // If latest_ekf_pose_ is older than seconds compared to current frame, it + // will not be published. + const rclcpp::Duration tolerance{ + static_cast(ekf_time_tolerance_), + static_cast((ekf_time_tolerance_ - std::floor(ekf_time_tolerance_)) * 1e9)}; + if (rclcpp::Time(latest_ekf_pose_.header.stamp) + tolerance < rclcpp::Time(msg.header.stamp)) { + RCLCPP_INFO( + this->get_logger(), + "latest_ekf_pose_ is older than %f seconds compared to current frame. " + "latest_ekf_pose_.header.stamp: %d.%d, msg.header.stamp: %d.%d", + ekf_time_tolerance_, latest_ekf_pose_.header.stamp.sec, latest_ekf_pose_.header.stamp.nanosec, + msg.header.stamp.sec, msg.header.stamp.nanosec); + return; + } + + // If curr_pose differs from latest_ekf_pose_ by more than , it will not + // be published. + const geometry_msgs::msg::Pose curr_pose = pose_with_covariance_stamped.pose.pose; + const geometry_msgs::msg::Pose latest_ekf_pose = latest_ekf_pose_.pose.pose; + const double diff_x = curr_pose.position.x - latest_ekf_pose.position.x; + const double diff_y = curr_pose.position.y - latest_ekf_pose.position.y; + const double diff_z = curr_pose.position.z - latest_ekf_pose.position.z; + const double diff_distance_squared = diff_x * diff_x + diff_y * diff_y + diff_z * diff_z; + const double threshold = ekf_position_tolerance_ * ekf_position_tolerance_; + if (threshold < diff_distance_squared) { + RCLCPP_INFO( + this->get_logger(), + "curr_pose differs from latest_ekf_pose_ by more than %f m. " + "curr_pose: (%f, %f, %f), latest_ekf_pose: (%f, %f, %f)", + ekf_position_tolerance_, curr_pose.position.x, curr_pose.position.y, curr_pose.position.z, + latest_ekf_pose.position.x, latest_ekf_pose.position.y, latest_ekf_pose.position.z); + return; + } + + // ~5[m]: base_covariance + // 5~[m]: scaling base_covariance by std::pow(distance/5, 3) + const double distance = std::sqrt(distance_squared); + const double scale = distance / 5; + const double coeff = std::max(1.0, std::pow(scale, 3)); + for (int i = 0; i < 36; i++) { + pose_with_covariance_stamped.pose.covariance[i] = coeff * base_covariance_[i]; + } pose_pub_->publish(pose_with_covariance_stamped); } diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index 6e7e194f7cf72..d5cd85aa2ed85 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -51,14 +51,7 @@ if(BUILD_TESTING) ) find_package(ament_cmake_gtest REQUIRED) - set(TEST_FILES - test/test_aged_object_queue.cpp - test/test_mahalanobis.cpp - test/test_measurement.cpp - test/test_numeric.cpp - test/test_state_transition.cpp - test/test_string.cpp - test/test_warning_message.cpp) + file(GLOB_RECURSE TEST_FILES test/*.cpp) foreach(filepath ${TEST_FILES}) add_testcase(${filepath}) diff --git a/localization/ekf_localizer/test/src/test_ekf_localizer.cpp b/localization/ekf_localizer/test/src/test_ekf_localizer.cpp deleted file mode 100644 index f249dffef6034..0000000000000 --- a/localization/ekf_localizer/test/src/test_ekf_localizer.cpp +++ /dev/null @@ -1,305 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "ekf_localizer/ekf_localizer.hpp" - -#include -#include -#include - -#include - -#include -#include - -#include -#include -#include -#include - -using std::placeholders::_1; - -class EKFLocalizerTestSuite : public ::testing::Test -{ -protected: - void SetUp() { rclcpp::init(0, nullptr); } - void TearDown() { (void)rclcpp::shutdown(); } -}; // sanity_check - -class TestEKFLocalizerNode : public EKFLocalizer -{ -public: - TestEKFLocalizerNode(const std::string & node_name, const rclcpp::NodeOptions & node_options) - : EKFLocalizer(node_name, node_options) - { - sub_twist = this->create_subscription( - "/ekf_twist", 1, std::bind(&TestEKFLocalizerNode::testCallbackTwist, this, _1)); - sub_pose = this->create_subscription( - "/ekf_pose", 1, std::bind(&TestEKFLocalizerNode::testCallbackPose, this, _1)); - - using std::chrono_literals::operator""ms; - test_timer_ = rclcpp::create_timer( - this, get_clock(), 100ms, std::bind(&TestEKFLocalizerNode::testTimerCallback, this)); - } - ~TestEKFLocalizerNode() {} - - std::string frame_id_a_ = "world"; - std::string frame_id_b_ = "base_link"; - - rclcpp::Subscription::SharedPtr sub_twist; - rclcpp::Subscription::SharedPtr sub_pose; - - rclcpp::TimerBase::SharedPtr test_timer_; - - geometry_msgs::msg::PoseStamped::SharedPtr test_current_pose_ptr_; - geometry_msgs::msg::TwistStamped::SharedPtr test_current_twist_ptr_; - - void testTimerCallback() - { - /* !!! this should be defined before sendTransform() !!! */ - static std::shared_ptr br = - std::make_shared(shared_from_this()); - geometry_msgs::msg::TransformStamped sent; - - rclcpp::Time current_time = this->now(); - - sent.header.stamp = current_time; - sent.header.frame_id = frame_id_a_; - sent.child_frame_id = frame_id_b_; - sent.transform.translation.x = -7.11; - sent.transform.translation.y = 0.0; - sent.transform.translation.z = 0.0; - tf2::Quaternion q; - q.setRPY(0, 0, 0.5); - sent.transform.rotation.x = q.x(); - sent.transform.rotation.y = q.y(); - sent.transform.rotation.z = q.z(); - sent.transform.rotation.w = q.w(); - - br->sendTransform(sent); - } - - void testCallbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose) - { - test_current_pose_ptr_ = std::make_shared(*pose); - } - - void testCallbackTwist(geometry_msgs::msg::TwistStamped::SharedPtr twist) - { - test_current_twist_ptr_ = std::make_shared(*twist); - } - - void resetCurrentPoseAndTwist() - { - test_current_pose_ptr_ = nullptr; - test_current_twist_ptr_ = nullptr; - } -}; - -TEST_F(EKFLocalizerTestSuite, measurementUpdatePose) -{ - rclcpp::NodeOptions node_options; - auto ekf = std::make_shared("EKFLocalizerTestSuite", node_options); - - auto pub_pose = ekf->create_publisher("/in_pose", 1); - - geometry_msgs::msg::PoseStamped in_pose; - in_pose.header.frame_id = "world"; - in_pose.pose.position.x = 1.0; - in_pose.pose.position.y = 2.0; - in_pose.pose.position.z = 3.0; - in_pose.pose.orientation.x = 0.0; - in_pose.pose.orientation.y = 0.0; - in_pose.pose.orientation.z = 0.0; - in_pose.pose.orientation.w = 1.0; - - /* test for valid value */ - const double pos_x = 12.3; - in_pose.pose.position.x = pos_x; // for valid value - - for (int i = 0; i < 20; ++i) { - in_pose.header.stamp = ekf->now(); - pub_pose->publish(in_pose); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ASSERT_NE(ekf->test_current_pose_ptr_, nullptr); - ASSERT_NE(ekf->test_current_twist_ptr_, nullptr); - - double ekf_x = ekf->test_current_pose_ptr_->pose.position.x; - bool is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - - ASSERT_TRUE(std::fabs(ekf_x - pos_x) < 0.1) - << "ekf pos x: " << ekf_x << " should be close to " << pos_x; - - /* test for invalid value */ - in_pose.pose.position.x = NAN; // check for invalid values - for (int i = 0; i < 10; ++i) { - in_pose.header.stamp = ekf->now(); - pub_pose->publish(in_pose); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - - ekf->resetCurrentPoseAndTwist(); -} - -TEST_F(EKFLocalizerTestSuite, measurementUpdateTwist) -{ - rclcpp::NodeOptions node_options; - auto ekf = std::make_shared("EKFLocalizerTestSuite", node_options); - - auto pub_twist = ekf->create_publisher("/in_twist", 1); - geometry_msgs::msg::TwistStamped in_twist; - in_twist.header.frame_id = "base_link"; - - /* test for valid value */ - const double vx = 12.3; - in_twist.twist.linear.x = vx; // for valid value - for (int i = 0; i < 20; ++i) { - in_twist.header.stamp = ekf->now(); - pub_twist->publish(in_twist); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ASSERT_FALSE(ekf->test_current_pose_ptr_ == nullptr); - ASSERT_FALSE(ekf->test_current_twist_ptr_ == nullptr); - - double ekf_vx = ekf->test_current_twist_ptr_->twist.linear.x; - bool is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - ASSERT_TRUE(std::fabs(ekf_vx - vx) < 0.1) - << "ekf vel x: " << ekf_vx << ", should be close to " << vx; - - /* test for invalid value */ - in_twist.twist.linear.x = NAN; // check for invalid values - for (int i = 0; i < 10; ++i) { - in_twist.header.stamp = ekf->now(); - pub_twist->publish(in_twist); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ekf_vx = ekf->test_current_twist_ptr_->twist.linear.x; - is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - - ekf->resetCurrentPoseAndTwist(); -} - -TEST_F(EKFLocalizerTestSuite, measurementUpdatePoseWithCovariance) -{ - rclcpp::NodeOptions node_options; - node_options.append_parameter_override("use_pose_with_covariance", true); - rclcpp::sleep_for(std::chrono::milliseconds(200)); - auto ekf = std::make_shared("EKFLocalizerTestSuite", node_options); - - auto pub_pose = ekf->create_publisher( - "/in_pose_with_covariance", 1); - geometry_msgs::msg::PoseWithCovarianceStamped in_pose; - in_pose.header.frame_id = "world"; - in_pose.pose.pose.position.x = 1.0; - in_pose.pose.pose.position.y = 2.0; - in_pose.pose.pose.position.z = 3.0; - in_pose.pose.pose.orientation.x = 0.0; - in_pose.pose.pose.orientation.y = 0.0; - in_pose.pose.pose.orientation.z = 0.0; - in_pose.pose.pose.orientation.w = 1.0; - for (int i = 0; i < 36; ++i) { - in_pose.pose.covariance[i] = 0.1; - } - - /* test for valid value */ - const double pos_x = 99.3; - in_pose.pose.pose.position.x = pos_x; // for valid value - - for (int i = 0; i < 20; ++i) { - in_pose.header.stamp = ekf->now(); - pub_pose->publish(in_pose); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ASSERT_FALSE(ekf->test_current_pose_ptr_ == nullptr); - ASSERT_FALSE(ekf->test_current_twist_ptr_ == nullptr); - - double ekf_x = ekf->test_current_pose_ptr_->pose.position.x; - bool is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - ASSERT_TRUE(std::fabs(ekf_x - pos_x) < 0.1) - << "ekf pos x: " << ekf_x << " should be close to " << pos_x; - - /* test for invalid value */ - in_pose.pose.pose.position.x = NAN; // check for invalid values - for (int i = 0; i < 10; ++i) { - in_pose.header.stamp = ekf->now(); - pub_pose->publish(in_pose); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - is_succeeded = !(std::isnan(ekf_x) || std::isinf(ekf_x)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - - ekf->resetCurrentPoseAndTwist(); -} - -TEST_F(EKFLocalizerTestSuite, measurementUpdateTwistWithCovariance) -{ - rclcpp::NodeOptions node_options; - auto ekf = std::make_shared("EKFLocalizerTestSuite", node_options); - - auto pub_twist = ekf->create_publisher( - "/in_twist_with_covariance", 1); - geometry_msgs::msg::TwistWithCovarianceStamped in_twist; - in_twist.header.frame_id = "base_link"; - - /* test for valid value */ - const double vx = 12.3; - in_twist.twist.twist.linear.x = vx; // for valid value - for (int i = 0; i < 36; ++i) { - in_twist.twist.covariance[i] = 0.1; - } - for (int i = 0; i < 10; ++i) { - in_twist.header.stamp = ekf->now(); - pub_twist->publish(in_twist); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ASSERT_FALSE(ekf->test_current_pose_ptr_ == nullptr); - ASSERT_FALSE(ekf->test_current_twist_ptr_ == nullptr); - - double ekf_vx = ekf->test_current_twist_ptr_->twist.linear.x; - bool is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; - ASSERT_TRUE((ekf_vx - vx) < 0.1) << "vel x should be close to " << vx; - - /* test for invalid value */ - in_twist.twist.twist.linear.x = NAN; // check for invalid values - for (int i = 0; i < 10; ++i) { - in_twist.header.stamp = ekf->now(); - pub_twist->publish(in_twist); - rclcpp::spin_some(ekf); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } - - ekf_vx = ekf->test_current_twist_ptr_->twist.linear.x; - is_succeeded = !(std::isnan(ekf_vx) || std::isinf(ekf_vx)); - ASSERT_EQ(true, is_succeeded) << "ekf result includes invalid value."; -} diff --git a/localization/ekf_localizer/test/test_covariance.cpp b/localization/ekf_localizer/test/test_covariance.cpp index fdc732ea6d34c..23458fb18a838 100644 --- a/localization/ekf_localizer/test/test_covariance.cpp +++ b/localization/ekf_localizer/test/test_covariance.cpp @@ -31,15 +31,15 @@ TEST(EKFCovarianceToPoseMessageCovariance, SmokeTest) P(2, 2) = 9.; std::array covariance = ekfCovarianceToPoseMessageCovariance(P); - EXPECT_EQ(covariance(0), 1.); - EXPECT_EQ(covariance(1), 2.); - EXPECT_EQ(covariance(5), 3.); - EXPECT_EQ(covariance(6), 4.); - EXPECT_EQ(covariance(7), 5.); - EXPECT_EQ(covariance(11), 6.); - EXPECT_EQ(covariance(30), 7.); - EXPECT_EQ(covariance(31), 8.); - EXPECT_EQ(covariance(35), 9.); + EXPECT_EQ(covariance[0], 1.); + EXPECT_EQ(covariance[1], 2.); + EXPECT_EQ(covariance[5], 3.); + EXPECT_EQ(covariance[6], 4.); + EXPECT_EQ(covariance[7], 5.); + EXPECT_EQ(covariance[11], 6.); + EXPECT_EQ(covariance[30], 7.); + EXPECT_EQ(covariance[31], 8.); + EXPECT_EQ(covariance[35], 9.); } // ensure other elements are zero @@ -62,10 +62,10 @@ TEST(EKFCovarianceToTwistMessageCovariance, SmokeTest) P(5, 5) = 4.; std::array covariance = ekfCovarianceToTwistMessageCovariance(P); - EXPECT_EQ(covariance(0), 1.); - EXPECT_EQ(covariance(5), 2.); - EXPECT_EQ(covariance(30), 3.); - EXPECT_EQ(covariance(35), 4.); + EXPECT_EQ(covariance[0], 1.); + EXPECT_EQ(covariance[5], 2.); + EXPECT_EQ(covariance[30], 3.); + EXPECT_EQ(covariance[35], 4.); } // ensure other elements are zero diff --git a/localization/ekf_localizer/test/test_ekf_localizer.test b/localization/ekf_localizer/test/test_ekf_localizer.test deleted file mode 100644 index 4eecfa6e094b7..0000000000000 --- a/localization/ekf_localizer/test/test_ekf_localizer.test +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index b30cbaca1e87a..fcbe4804ca7d2 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -64,6 +64,7 @@ One optional function is regularization. Please see the regularization chapter i | `max_iterations` | int | The number of iterations required to calculate alignment | | `converged_param_type` | int | The type of indicators for scan matching score (0: TP, 1: NVTL) | | `converged_param_transform_probability` | double | Threshold for deciding whether to trust the estimation result | +| `lidar_topic_timeout_sec` | double | Tolerance of timestamp difference between current time and sensor pointcloud | | `num_threads` | int | Number of threads used for parallel computing | (TP: Transform Probability, NVTL: Nearest Voxel Transform Probability) diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index a5d8142b6616e..b5affd72b2158 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -37,6 +37,9 @@ # The number of particles to estimate initial pose initial_estimate_particles_num: 100 + # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] + lidar_topic_timeout_sec: 1.0 + # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] initial_pose_timeout_sec: 1.0 diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 8657e354c728a..ed3b99019f7d9 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -179,6 +179,7 @@ class NDTScanMatcher : public rclcpp::Node double converged_param_nearest_voxel_transformation_likelihood_; int initial_estimate_particles_num_; + double lidar_topic_timeout_sec_; double initial_pose_timeout_sec_; double initial_pose_distance_tolerance_m_; float inversion_vector_threshold_; diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 667fd59196df3..103df19c2b5db 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -91,6 +91,7 @@ NDTScanMatcher::NDTScanMatcher() converged_param_transform_probability_(4.5), converged_param_nearest_voxel_transformation_likelihood_(2.3), initial_estimate_particles_num_(100), + lidar_topic_timeout_sec_(1.0), initial_pose_timeout_sec_(1.0), initial_pose_distance_tolerance_m_(10.0), inversion_vector_threshold_(-0.9), @@ -141,6 +142,9 @@ NDTScanMatcher::NDTScanMatcher() "converged_param_nearest_voxel_transformation_likelihood", converged_param_nearest_voxel_transformation_likelihood_); + lidar_topic_timeout_sec_ = + this->declare_parameter("lidar_topic_timeout_sec", lidar_topic_timeout_sec_); + initial_pose_timeout_sec_ = this->declare_parameter("initial_pose_timeout_sec", initial_pose_timeout_sec_); @@ -268,9 +272,16 @@ void NDTScanMatcher::timer_diagnostic() diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "Initializing State. "; } + if ( + state_ptr_->count("lidar_topic_delay_time_sec") && + std::stod((*state_ptr_)["lidar_topic_delay_time_sec"]) > lidar_topic_timeout_sec_) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_status_msg.message += "lidar_topic_delay_time_sec exceed limit. "; + } if ( state_ptr_->count("skipping_publish_num") && - std::stoi((*state_ptr_)["skipping_publish_num"]) > 1) { + std::stoi((*state_ptr_)["skipping_publish_num"]) > 1 && + std::stoi((*state_ptr_)["skipping_publish_num"]) < 5) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "skipping_publish_num > 1. "; } @@ -280,6 +291,13 @@ void NDTScanMatcher::timer_diagnostic() diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; diag_status_msg.message += "skipping_publish_num exceed limit. "; } + if ( + state_ptr_->count("nearest_voxel_transformation_likelihood") && + std::stod((*state_ptr_)["nearest_voxel_transformation_likelihood"]) < + converged_param_nearest_voxel_transformation_likelihood_) { + diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_status_msg.message += "NDT score is unreliably low. "; + } // Ignore local optimal solution if ( state_ptr_->count("is_local_optimal_solution_oscillation") && @@ -346,11 +364,29 @@ void NDTScanMatcher::callback_sensor_points( return; } + const rclcpp::Time sensor_ros_time = sensor_points_msg_in_sensor_frame->header.stamp; + const double lidar_topic_delay_time_sec = (this->now() - sensor_ros_time).seconds(); + (*state_ptr_)["lidar_topic_delay_time_sec"] = std::to_string(lidar_topic_delay_time_sec); + + if (lidar_topic_delay_time_sec > lidar_topic_timeout_sec_) { + RCLCPP_WARN( + this->get_logger(), + "The LiDAR topic is experiencing latency. The delay time is %lf[sec] (the tolerance is " + "%lf[sec])", + lidar_topic_delay_time_sec, lidar_topic_timeout_sec_); + + // If the delay time of the LiDAR topic exceeds the delay compensation time of ekf_localizer, + // even if further processing continues, the estimated result will be rejected by ekf_localizer. + // Therefore, it would be acceptable to exit the function here. + // However, for now, we will continue the processing as it is. + + // return; + } + // mutex ndt_ptr_ std::lock_guard lock(ndt_ptr_mtx_); const auto exe_start_time = std::chrono::system_clock::now(); - const rclcpp::Time sensor_ros_time = sensor_points_msg_in_sensor_frame->header.stamp; // preprocess input pointcloud pcl::shared_ptr> sensor_points_in_sensor_frame( diff --git a/perception/cluster_merger/CMakeLists.txt b/perception/cluster_merger/CMakeLists.txt new file mode 100644 index 0000000000000..49506f4b439fb --- /dev/null +++ b/perception/cluster_merger/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.8) +project(cluster_merger) + +# find dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +# Targets +ament_auto_add_library(cluster_merger_node_component SHARED + src/cluster_merger/node.cpp +) + +rclcpp_components_register_node(cluster_merger_node_component + PLUGIN "cluster_merger::ClusterMergerNode" + EXECUTABLE cluster_merger_node) + + +if(BUILD_TESTING) + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/perception/cluster_merger/README.md b/perception/cluster_merger/README.md new file mode 100644 index 0000000000000..b46f7401b70ec --- /dev/null +++ b/perception/cluster_merger/README.md @@ -0,0 +1,72 @@ +# cluster merger + +## Purpose + +cluster_merger is a package for merging pointcloud clusters as detected objects with feature type. + +## Inner-working / Algorithms + +The clusters of merged topics are simply concatenated from clusters of input topics. + +## Input / Output + +### Input + +| Name | Type | Description | +| ---------------- | -------------------------------------------------------- | ------------------- | +| `input/cluster0` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | pointcloud clusters | +| `input/cluster1` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | pointcloud clusters | + +### Output + +| Name | Type | Description | +| ----------------- | ----------------------------------------------------- | --------------- | +| `output/clusters` | `autoware_auto_perception_msgs::msg::DetectedObjects` | merged clusters | + +## Parameters + +| Name | Type | Description | Default value | +| :---------------- | :----- | :----------------------------------- | :------------ | +| `output_frame_id` | string | The header frame_id of output topic. | base_link | + +## Assumptions / Known limits + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts diff --git a/perception/cluster_merger/include/cluster_merger/node.hpp b/perception/cluster_merger/include/cluster_merger/node.hpp new file mode 100644 index 0000000000000..8da5999f00384 --- /dev/null +++ b/perception/cluster_merger/include/cluster_merger/node.hpp @@ -0,0 +1,73 @@ +// 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. + +#ifndef CLUSTER_MERGER__NODE_HPP_ +#define CLUSTER_MERGER__NODE_HPP_ + +#include "message_filters/subscriber.h" +#include "message_filters/sync_policies/approximate_time.h" +#include "message_filters/synchronizer.h" +#include "rclcpp/rclcpp.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include "tier4_perception_msgs/msg/detected_objects_with_feature.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace cluster_merger +{ +using tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using tier4_perception_msgs::msg::DetectedObjectWithFeature; + +class ClusterMergerNode : public rclcpp::Node +{ +public: + explicit ClusterMergerNode(const rclcpp::NodeOptions & node_options); + +private: + // Subscriber + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + rclcpp::Subscription::SharedPtr sub_objects_{}; + message_filters::Subscriber objects0_sub_; + message_filters::Subscriber objects1_sub_; + typedef message_filters::sync_policies::ApproximateTime< + DetectedObjectsWithFeature, DetectedObjectsWithFeature> + SyncPolicy; + typedef message_filters::Synchronizer Sync; + Sync sync_; + + std::string output_frame_id_; + + std::vector::SharedPtr> sub_objects_array{}; + std::shared_ptr transform_listener_; + + void objectsCallback( + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects0_msg, + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects1_msg); + // Publisher + rclcpp::Publisher::SharedPtr pub_objects_; +}; + +} // namespace cluster_merger + +#endif // CLUSTER_MERGER__NODE_HPP_ diff --git a/perception/cluster_merger/launch/cluster_merger.launch.xml b/perception/cluster_merger/launch/cluster_merger.launch.xml new file mode 100644 index 0000000000000..1bbd0ebd91e12 --- /dev/null +++ b/perception/cluster_merger/launch/cluster_merger.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/perception/cluster_merger/package.xml b/perception/cluster_merger/package.xml new file mode 100644 index 0000000000000..14826ad07e098 --- /dev/null +++ b/perception/cluster_merger/package.xml @@ -0,0 +1,28 @@ + + + + cluster_merger + 0.1.0 + The ROS 2 cluster merger package + Yukihiro Saito + Shunsuke Miura + autoware + Apache License 2.0 + + ament_cmake_auto + + geometry_msgs + message_filters + object_recognition_utils + rclcpp + rclcpp_components + tier4_autoware_utils + tier4_perception_msgs + autoware_cmake + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/cluster_merger/src/cluster_merger/node.cpp b/perception/cluster_merger/src/cluster_merger/node.cpp new file mode 100644 index 0000000000000..48bf953027510 --- /dev/null +++ b/perception/cluster_merger/src/cluster_merger/node.cpp @@ -0,0 +1,73 @@ +// 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. + +#include "cluster_merger/node.hpp" + +#include "object_recognition_utils/object_recognition_utils.hpp" + +#include +#include +#include +namespace cluster_merger +{ + +ClusterMergerNode::ClusterMergerNode(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("cluster_merger_node", node_options), + tf_buffer_(get_clock()), + tf_listener_(tf_buffer_), + objects0_sub_(this, "input/cluster0", rclcpp::QoS{1}.get_rmw_qos_profile()), + objects1_sub_(this, "input/cluster1", rclcpp::QoS{1}.get_rmw_qos_profile()), + sync_(SyncPolicy(10), objects0_sub_, objects1_sub_) +{ + output_frame_id_ = declare_parameter("output_frame_id"); + using std::placeholders::_1; + using std::placeholders::_2; + sync_.registerCallback(std::bind(&ClusterMergerNode::objectsCallback, this, _1, _2)); + + // Publisher + pub_objects_ = create_publisher("~/output/clusters", rclcpp::QoS{1}); +} + +void ClusterMergerNode::objectsCallback( + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects0_msg, + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects1_msg) +{ + if (pub_objects_->get_subscription_count() < 1) { + return; + } + // TODO(badai-nguyen): transform input topics to desired frame before concatenating + /* transform to the same with cluster0 frame id*/ + DetectedObjectsWithFeature transformed_objects0; + DetectedObjectsWithFeature transformed_objects1; + if ( + !object_recognition_utils::transformObjectsWithFeature( + *input_objects0_msg, output_frame_id_, tf_buffer_, transformed_objects0) || + !object_recognition_utils::transformObjectsWithFeature( + *input_objects1_msg, output_frame_id_, tf_buffer_, transformed_objects1)) { + return; + } + + DetectedObjectsWithFeature output_objects; + output_objects.header = input_objects0_msg->header; + // add check frame id and transform if they are different + output_objects.feature_objects = transformed_objects0.feature_objects; + output_objects.feature_objects.insert( + output_objects.feature_objects.end(), transformed_objects1.feature_objects.begin(), + transformed_objects1.feature_objects.end()); + pub_objects_->publish(output_objects); +} +} // namespace cluster_merger + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(cluster_merger::ClusterMergerNode) diff --git a/perception/crosswalk_traffic_light_estimator/CMakeLists.txt b/perception/crosswalk_traffic_light_estimator/CMakeLists.txt index 11a6b232caf90..2947070eca01e 100644 --- a/perception/crosswalk_traffic_light_estimator/CMakeLists.txt +++ b/perception/crosswalk_traffic_light_estimator/CMakeLists.txt @@ -28,6 +28,8 @@ endif() ## Install ## ############# -ament_auto_package(INSTALL_TO_SHARE - launch +ament_auto_package( + INSTALL_TO_SHARE + launch + config ) diff --git a/perception/crosswalk_traffic_light_estimator/README.md b/perception/crosswalk_traffic_light_estimator/README.md index e23b454905081..73c7151997c54 100644 --- a/perception/crosswalk_traffic_light_estimator/README.md +++ b/perception/crosswalk_traffic_light_estimator/README.md @@ -22,9 +22,10 @@ ## Parameters -| Name | Type | Description | Default value | -| :---------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| `use_last_detect_color` | `bool` | If this parameter is `true`, this module estimates pedestrian's traffic signal as RED not only when vehicle's traffic signal is detected as GREEN/AMBER but also when detection results change GREEN/AMBER to UNKNOWN. (If detection results change RED or AMBER to UNKNOWN, this module estimates pedestrian's traffic signal as UNKNOWN.) If this parameter is `false`, this module use only latest detection results for estimation. (Only when the detection result is GREEN/AMBER, this module estimates pedestrian's traffic signal as RED.) | `true` | +| Name | Type | Description | Default value | +| :---------------------------- | :------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `use_last_detect_color` | `bool` | If this parameter is `true`, this module estimates pedestrian's traffic signal as RED not only when vehicle's traffic signal is detected as GREEN/AMBER but also when detection results change GREEN/AMBER to UNKNOWN. (If detection results change RED or AMBER to UNKNOWN, this module estimates pedestrian's traffic signal as UNKNOWN.) If this parameter is `false`, this module use only latest detection results for estimation. (Only when the detection result is GREEN/AMBER, this module estimates pedestrian's traffic signal as RED.) | `true` | +| `last_detect_color_hold_time` | `double` | The time threshold to hold for last detect color. | `2.0` | ## Inner-workings / Algorithms diff --git a/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml b/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml new file mode 100644 index 0000000000000..33168c87de7eb --- /dev/null +++ b/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml @@ -0,0 +1,18 @@ +# Copyright 2023 UCI SORA Lab +# +# 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. + +/**: + ros__parameters: + use_last_detect_color: true + last_detect_color_hold_time: 2.0 diff --git a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp index 161a4d19b96ef..e38b747a6ce67 100644 --- a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp +++ b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp @@ -15,9 +15,6 @@ #ifndef CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_ #define CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_ -#include -#include -#include #include #include #include @@ -32,6 +29,7 @@ #include #include +#include #include #include #include diff --git a/perception/crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml b/perception/crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml index 91bec60c90aee..21d718c3439cd 100644 --- a/perception/crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml +++ b/perception/crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml @@ -1,9 +1,23 @@ - + + + + diff --git a/perception/crosswalk_traffic_light_estimator/package.xml b/perception/crosswalk_traffic_light_estimator/package.xml index f029972f7124f..bec5b666683c4 100644 --- a/perception/crosswalk_traffic_light_estimator/package.xml +++ b/perception/crosswalk_traffic_light_estimator/package.xml @@ -5,6 +5,8 @@ The crosswalk_traffic_light_estimator package Satoshi Ota + Shunsuke Miura + Tao Zhong Apache License 2.0 ament_cmake_auto diff --git a/perception/crosswalk_traffic_light_estimator/schema/crosswalk_traffic_light_estimator.schema.json b/perception/crosswalk_traffic_light_estimator/schema/crosswalk_traffic_light_estimator.schema.json new file mode 100644 index 0000000000000..ecc273263236a --- /dev/null +++ b/perception/crosswalk_traffic_light_estimator/schema/crosswalk_traffic_light_estimator.schema.json @@ -0,0 +1,36 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Crosswalk Traffic Light Estimator Node", + "type": "object", + "definitions": { + "crosswalk_traffic_light_estimator": { + "type": "object", + "properties": { + "use_last_detect_color": { + "type": "boolean", + "default": true, + "description": "If this parameter is true, this module estimates pedestrian's traffic signal as RED not only when vehicle's traffic signal is detected as GREEN/AMBER but also when detection results change GREEN/AMBER to UNKNOWN. (If detection results change RED or AMBER to UNKNOWN, this module estimates pedestrian's traffic signal as UNKNOWN.) If this parameter is false, this module use only latest detection results for estimation. (Only when the detection result is GREEN/AMBER, this module estimates pedestrian's traffic signal as RED.)." + }, + "last_detect_color_hold_time": { + "type": "number", + "default": 2.0, + "exclusiveMinimum": 0.0, + "description": "The time threshold to hold for last detect color." + } + }, + "required": ["use_last_detect_color", "last_detect_color_hold_time"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/crosswalk_traffic_light_estimator" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/crosswalk_traffic_light_estimator/src/node.cpp b/perception/crosswalk_traffic_light_estimator/src/node.cpp index dd7a9ea04ae38..e45e3494dadce 100644 --- a/perception/crosswalk_traffic_light_estimator/src/node.cpp +++ b/perception/crosswalk_traffic_light_estimator/src/node.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 TIER IV, Inc. +// Copyright 2022-2023 UCI SORA Lab, TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,6 +13,7 @@ // limitations under the License. #include "crosswalk_traffic_light_estimator/node.hpp" +#include #include #include @@ -81,8 +82,8 @@ CrosswalkTrafficLightEstimatorNode::CrosswalkTrafficLightEstimatorNode( { using std::placeholders::_1; - use_last_detect_color_ = this->declare_parameter("use_last_detect_color", true); - last_detect_color_hold_time_ = this->declare_parameter("last_detect_color_hold_time", 2.0); + use_last_detect_color_ = declare_parameter("use_last_detect_color"); + last_detect_color_hold_time_ = declare_parameter("last_detect_color_hold_time"); sub_map_ = create_subscription( "~/input/vector_map", rclcpp::QoS{1}.transient_local(), diff --git a/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp index d1d3cf9e904c5..600da31df5868 100644 --- a/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp @@ -17,14 +17,13 @@ #include "utils/utils.hpp" -#include -#include #include #include #include #include +#include #include #include diff --git a/perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp b/perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp index b3ab5d104aee7..b9384e0f70379 100644 --- a/perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp @@ -17,8 +17,6 @@ #include "utils/utils.hpp" -#include -#include #include #include diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index 2e393f27c37a7..3b78ae449e2da 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -14,6 +14,8 @@ #include "detected_object_filter/object_lanelet_filter.hpp" +#include +#include #include #include diff --git a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp index 3f383ce3d6d0e..7c0609ee1cdce 100644 --- a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp +++ b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp @@ -19,8 +19,6 @@ #include #include #include -#include -#include #include #include @@ -30,6 +28,7 @@ #include #include +#include #include #include diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp index 096e084a70286..425af519a80ea 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -20,6 +20,8 @@ #include #include #include +#include +#include #include #include diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index 906ad71d21732..ce7c3b5ea12a9 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -22,6 +22,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/utils/utils.cpp src/roi_cluster_fusion/node.cpp src/roi_detected_object_fusion/node.cpp + src/roi_pointcloud_fusion/node.cpp ) target_link_libraries(${PROJECT_NAME} @@ -39,6 +40,11 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE roi_cluster_fusion_node ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "image_projection_based_fusion::RoiPointCloudFusionNode" + EXECUTABLE roi_pointcloud_fusion_node +) + set(CUDA_VERBOSE OFF) # set flags for CUDA availability diff --git a/perception/image_projection_based_fusion/README.md b/perception/image_projection_based_fusion/README.md index 1bbee35ab44f8..207989d8a6f25 100644 --- a/perception/image_projection_based_fusion/README.md +++ b/perception/image_projection_based_fusion/README.md @@ -58,3 +58,4 @@ The rclcpp::TimerBase timer could not break a for loop, therefore even if time i | roi_cluster_fusion | Overwrite a classification label of clusters by that of ROIs from a 2D object detector. | [link](./docs/roi-cluster-fusion.md) | | roi_detected_object_fusion | Overwrite a classification label of detected objects by that of ROIs from a 2D object detector. | [link](./docs/roi-detected-object-fusion.md) | | pointpainting_fusion | Paint the point cloud with the ROIs from a 2D object detector and feed to a 3D object detector. | [link](./docs/pointpainting-fusion.md) | +| roi_pointcloud_fusion | Matching pointcloud with ROIs from a 2D object detector to detect unknown-labeled objects | [link](./docs/roi-pointcloud-fusion.md) | diff --git a/perception/image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png b/perception/image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png new file mode 100644 index 0000000000000..c529de7c728fb Binary files /dev/null and b/perception/image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png differ diff --git a/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md b/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md new file mode 100644 index 0000000000000..5d0b241a578d6 --- /dev/null +++ b/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md @@ -0,0 +1,93 @@ +# roi_pointcloud_fusion + +## Purpose + +The node `roi_pointcloud_fusion` is to cluster the pointcloud based on Region Of Interests (ROIs) detected by a 2D object detector, specific for unknown labeled ROI. + +## Inner-workings / Algorithms + +- The pointclouds are projected onto image planes and extracted as cluster if they are inside the unknown labeled ROIs. +- Since the cluster might contain unrelated points from background, then a refinement step is added to filter the background pointcloud by distance to camera. + +![roi_pointcloud_fusion_image](./images/roi_pointcloud_fusion.png) + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------ | -------------------------------------------------------- | --------------------------------------------------------- | +| `input` | `sensor_msgs::msg::PointCloud2` | input pointcloud | +| `input/camera_info[0-7]` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes | +| `input/rois[0-7]` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | ROIs from each image | +| `input/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | + +### Output + +| Name | Type | Description | +| ----------------- | -------------------------------------------------------- | -------------------------------------------- | +| `output` | `sensor_msgs::msg::PointCloud2` | output pointcloud as default of interface | +| `output_clusters` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | output clusters | +| `debug/clusters` | `sensor_msgs/msg/PointCloud2` | colored cluster pointcloud for visualization | + +## Parameters + +### Core Parameters + +| Name | Type | Description | +| ---------------------- | ------ | -------------------------------------------------------------------------------------------- | +| `min_cluster_size` | int | the minimum number of points that a cluster needs to contain in order to be considered valid | +| `cluster_2d_tolerance` | double | cluster tolerance measured in radial direction | +| `rois_number` | int | the number of input rois | + +## Assumptions / Known limits + +- Currently, the refinement is only based on distance to camera, the roi based clustering is supposed to work well with small object ROIs. Others criteria for refinement might needed in the future. + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts + + diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp index 016633fa4ef92..5174aebe069a8 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp @@ -29,6 +29,9 @@ #include #include #include +#include +#include +#include #include #include @@ -49,6 +52,8 @@ using autoware_auto_perception_msgs::msg::DetectedObjects; using sensor_msgs::msg::PointCloud2; using tier4_perception_msgs::msg::DetectedObjectsWithFeature; using tier4_perception_msgs::msg::DetectedObjectWithFeature; +using PointCloud = pcl::PointCloud; +using autoware_auto_perception_msgs::msg::ObjectClassification; template class FusionNode : public rclcpp::Node diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp index 3e223dd3101df..5e7a5087efb84 100755 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp @@ -17,6 +17,7 @@ #include +#include #include namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp index 38f9e71eea02d..70a7866e79b87 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -43,13 +43,13 @@ class RoiClusterFusionNode bool use_iou_{false}; bool use_cluster_semantic_type_{false}; bool only_allow_inside_cluster_{false}; - float roi_scale_factor_{1.1f}; - float iou_threshold_{0.0f}; - float unknown_iou_threshold_{0.0f}; + double roi_scale_factor_{1.1}; + double iou_threshold_{0.0}; + double unknown_iou_threshold_{0.0}; const float min_roi_existence_prob_ = 0.1; // keep small value to lessen affect on merger object stage bool remove_unknown_; - float trust_distance_; + double trust_distance_; bool filter_by_distance(const DetectedObjectWithFeature & obj); bool out_of_scope(const DetectedObjectWithFeature & obj); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp new file mode 100644 index 0000000000000..2b4eb822a9edb --- /dev/null +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -0,0 +1,53 @@ +// 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. + +#ifndef IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ +#define IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ + +#include "image_projection_based_fusion/fusion_node.hpp" + +#include +#include +namespace image_projection_based_fusion +{ +class RoiPointCloudFusionNode +: public FusionNode +{ +private: + int min_cluster_size_{1}; + bool fuse_unknown_only_{true}; + double cluster_2d_tolerance_; + + rclcpp::Publisher::SharedPtr pub_objects_ptr_; + std::vector output_fused_objects_; + rclcpp::Publisher::SharedPtr cluster_debug_pub_; + + /* data */ +public: + explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options); + +protected: + void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + + void postprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + + void fuseOnSingleImage( + const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, + const DetectedObjectsWithFeature & input_roi_msg, + const sensor_msgs::msg::CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override; + bool out_of_scope(const DetectedObjectWithFeature & obj); +}; + +} // namespace image_projection_based_fusion +#endif // IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp index 541cf997412c2..d7fd3c3882046 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp @@ -32,18 +32,54 @@ #include #endif +#include "image_projection_based_fusion/fusion_node.hpp" + +#include +#include +#include +#include + +#include "autoware_auto_perception_msgs/msg/shape.hpp" +#include "tier4_perception_msgs/msg/detected_object_with_feature.hpp" + +#include +#include +#include + #include #include +#include namespace image_projection_based_fusion { +using PointCloud = pcl::PointCloud; std::optional getTransformStamped( const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, const std::string & source_frame_id, const rclcpp::Time & time); Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t); +void convertCluster2FeatureObject( + const std_msgs::msg::Header & header, const PointCloud & cluster, + DetectedObjectWithFeature & feature_obj); +PointCloud closest_cluster( + PointCloud & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center); + +void updateOutputFusedObjects( + std::vector & output_objs, const std::vector & clusters, + const std_msgs::msg::Header & in_cloud_header, const std_msgs::msg::Header & in_roi_header, + const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const float cluster_2d_tolerance, + std::vector & output_fused_objects); + +geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); + +pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster); +void addShapeAndKinematic( + const pcl::PointCloud & cluster, + tier4_perception_msgs::msg::DetectedObjectWithFeature & feature_obj); + } // namespace image_projection_based_fusion #endif // IMAGE_PROJECTION_BASED_FUSION__UTILS__UTILS_HPP_ diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index 37fbd309d3ee5..2d99c25bb68f7 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -48,9 +48,10 @@ + - + diff --git a/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml new file mode 100644 index 0000000000000..181f4ccb88320 --- /dev/null +++ b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index 32d7a5633b811..5ff99af2ebb21 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -14,6 +14,7 @@ autoware_auto_perception_msgs autoware_point_types cv_bridge + euclidean_cluster image_transport lidar_centerpoint message_filters diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index 1bc351b08c01b..a540688f7e751 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -422,4 +422,5 @@ void FusionNode::publish(const Msg & output_msg) template class FusionNode; template class FusionNode; template class FusionNode; +template class FusionNode; } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 5253ac192c786..9f8ef7b94a123 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -133,7 +133,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt auto it = find(paint_class_names.begin(), paint_class_names.end(), cls); if (it != paint_class_names.end()) { int index = it - paint_class_names.begin(); - class_index_[cls] = index + 1; + class_index_[cls] = pow(2, index); // regard each class as a bit in binary } else { isClassTable_.erase(cls); } @@ -345,7 +345,8 @@ dc | dc dc dc dc ||zc| data = &painted_pointcloud_msg.data[0]; auto p_class = reinterpret_cast(&output[stride + class_offset]); for (const auto & cls : isClassTable_) { - *p_class = cls.second(label2d) ? class_index_[cls.first] : *p_class; + // add up the class values if the point belongs to multiple classes + *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; } } #if 0 diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp index bd49d9e446f1f..f462b9b0ba17a 100755 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp @@ -59,8 +59,10 @@ std::size_t VoxelGenerator::pointsToVoxels( point[1] = point_current.y(); point[2] = point_current.z(); point[3] = time_lag; + // decode the class value back to one-hot binary and assign it to point for (std::size_t i = 1; i <= config_.class_size_; i++) { - point[3 + i] = (*class_iter == i) ? 1 : 0; + auto decode = std::bitset<8>(*class_iter).to_string(); + point[3 + i] = decode[-i] == 1 ? 1 : 0; } out_of_range = false; diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 9180b18adeed8..e9cb76b050427 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -36,16 +36,16 @@ namespace image_projection_based_fusion RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) : FusionNode("roi_cluster_fusion", options) { - use_iou_x_ = declare_parameter("use_iou_x", true); - use_iou_y_ = declare_parameter("use_iou_y", false); - use_iou_ = declare_parameter("use_iou", false); - use_cluster_semantic_type_ = declare_parameter("use_cluster_semantic_type", false); - only_allow_inside_cluster_ = declare_parameter("only_allow_inside_cluster_", true); - roi_scale_factor_ = declare_parameter("roi_scale_factor", 1.1); - iou_threshold_ = declare_parameter("iou_threshold", 0.1); - unknown_iou_threshold_ = declare_parameter("unknown_iou_threshold", 0.1); - remove_unknown_ = declare_parameter("remove_unknown", false); - trust_distance_ = declare_parameter("trust_distance", 100.0); + use_iou_x_ = declare_parameter("use_iou_x"); + use_iou_y_ = declare_parameter("use_iou_y"); + use_iou_ = declare_parameter("use_iou"); + use_cluster_semantic_type_ = declare_parameter("use_cluster_semantic_type"); + only_allow_inside_cluster_ = declare_parameter("only_allow_inside_cluster"); + roi_scale_factor_ = declare_parameter("roi_scale_factor"); + iou_threshold_ = declare_parameter("iou_threshold"); + unknown_iou_threshold_ = declare_parameter("unknown_iou_threshold"); + remove_unknown_ = declare_parameter("remove_unknown"); + trust_distance_ = declare_parameter("trust_distance"); } void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) @@ -171,12 +171,16 @@ void RoiClusterFusionNode::fuseOnSingleImage( for (const auto & feature_obj : input_roi_msg.feature_objects) { int index = 0; double max_iou = 0.0; + bool is_roi_label_known = + feature_obj.object.classification.front().label != ObjectClassification::UNKNOWN; for (const auto & cluster_map : m_cluster_roi) { double iou(0.0), iou_x(0.0), iou_y(0.0); if (use_iou_) { iou = calcIoU(cluster_map.second, feature_obj.feature.roi); } - if (use_iou_x_) { + // use for unknown roi to improve small objects like traffic cone detect + // TODO(badai-nguyen): add option to disable roi_cluster mode + if (use_iou_x_ || !is_roi_label_known) { iou_x = calcIoUX(cluster_map.second, feature_obj.feature.roi); } if (use_iou_y_) { @@ -192,8 +196,6 @@ void RoiClusterFusionNode::fuseOnSingleImage( } } if (!output_cluster_msg.feature_objects.empty()) { - bool is_roi_label_known = feature_obj.object.classification.front().label != - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; bool is_roi_existence_prob_higher = output_cluster_msg.feature_objects.at(index).object.existence_probability <= feature_obj.object.existence_probability; diff --git a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp new file mode 100644 index 0000000000000..233e568ebee0b --- /dev/null +++ b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -0,0 +1,165 @@ +// 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. + +#include "image_projection_based_fusion/roi_pointcloud_fusion/node.hpp" + +#include "image_projection_based_fusion/utils/geometry.hpp" +#include "image_projection_based_fusion/utils/utils.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include +#include +#endif + +#include "euclidean_cluster/utils.hpp" + +namespace image_projection_based_fusion +{ +RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) +: FusionNode("roi_pointcloud_fusion", options) +{ + fuse_unknown_only_ = declare_parameter("fuse_unknown_only"); + min_cluster_size_ = declare_parameter("min_cluster_size"); + cluster_2d_tolerance_ = declare_parameter("cluster_2d_tolerance"); + pub_objects_ptr_ = + this->create_publisher("output_clusters", rclcpp::QoS{1}); + cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); +} + +void RoiPointCloudFusionNode::preprocess(__attribute__((unused)) + sensor_msgs::msg::PointCloud2 & pointcloud_msg) +{ + return; +} + +void RoiPointCloudFusionNode::postprocess(__attribute__((unused)) + sensor_msgs::msg::PointCloud2 & pointcloud_msg) +{ + const auto objects_sub_count = pub_objects_ptr_->get_subscription_count() + + pub_objects_ptr_->get_intra_process_subscription_count(); + if (objects_sub_count < 1) { + return; + } + + DetectedObjectsWithFeature output_msg; + output_msg.header = pointcloud_msg.header; + output_msg.feature_objects = output_fused_objects_; + + if (objects_sub_count > 0) { + pub_objects_ptr_->publish(output_msg); + } + output_fused_objects_.clear(); + // publish debug cluster + if (cluster_debug_pub_->get_subscription_count() > 0) { + sensor_msgs::msg::PointCloud2 debug_cluster_msg; + euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); + cluster_debug_pub_->publish(debug_cluster_msg); + } +} +void RoiPointCloudFusionNode::fuseOnSingleImage( + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, + __attribute__((unused)) const std::size_t image_id, + const DetectedObjectsWithFeature & input_roi_msg, + const sensor_msgs::msg::CameraInfo & camera_info, + __attribute__((unused)) sensor_msgs::msg::PointCloud2 & output_pointcloud_msg) +{ + if (input_pointcloud_msg.data.empty()) { + return; + } + std::vector output_objs; + // select ROIs for fusion + for (const auto & feature_obj : input_roi_msg.feature_objects) { + if (fuse_unknown_only_) { + bool is_roi_label_unknown = feature_obj.object.classification.front().label == + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + if (is_roi_label_unknown) { + output_objs.push_back(feature_obj); + } + } else { + // TODO(badai-nguyen): selected class from a list + output_objs.push_back(feature_obj); + } + } + if (output_objs.empty()) { + return; + } + + // transform pointcloud to camera optical frame id + Eigen::Matrix4d projection; + projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3), + camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7), + camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11); + geometry_msgs::msg::TransformStamped transform_stamped; + { + const auto transform_stamped_optional = getTransformStamped( + tf_buffer_, input_roi_msg.header.frame_id, input_pointcloud_msg.header.frame_id, + input_roi_msg.header.stamp); + if (!transform_stamped_optional) { + return; + } + transform_stamped = transform_stamped_optional.value(); + } + + sensor_msgs::msg::PointCloud2 transformed_cloud; + tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); + + std::vector clusters; + clusters.resize(output_objs.size()); + + for (sensor_msgs::PointCloud2ConstIterator iter_x(transformed_cloud, "x"), + iter_y(transformed_cloud, "y"), iter_z(transformed_cloud, "z"), + iter_orig_x(input_pointcloud_msg, "x"), iter_orig_y(input_pointcloud_msg, "y"), + iter_orig_z(input_pointcloud_msg, "z"); + iter_x != iter_x.end(); + ++iter_x, ++iter_y, ++iter_z, ++iter_orig_x, ++iter_orig_y, ++iter_orig_z) { + if (*iter_z <= 0.0) { + continue; + } + Eigen::Vector4d projected_point = projection * Eigen::Vector4d(*iter_x, *iter_y, *iter_z, 1.0); + Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( + projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); + + for (std::size_t i = 0; i < output_objs.size(); ++i) { + auto & feature_obj = output_objs.at(i); + const auto & check_roi = feature_obj.feature.roi; + auto & cluster = clusters.at(i); + + if ( + check_roi.x_offset <= normalized_projected_point.x() && + check_roi.y_offset <= normalized_projected_point.y() && + check_roi.x_offset + check_roi.width >= normalized_projected_point.x() && + check_roi.y_offset + check_roi.height >= normalized_projected_point.y()) { + cluster.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + } + } + } + + // refine and update output_fused_objects_ + updateOutputFusedObjects( + output_objs, clusters, input_pointcloud_msg.header, input_roi_msg.header, tf_buffer_, + min_cluster_size_, cluster_2d_tolerance_, output_fused_objects_); +} + +bool RoiPointCloudFusionNode::out_of_scope(__attribute__((unused)) + const DetectedObjectWithFeature & obj) +{ + return false; +} +} // namespace image_projection_based_fusion + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(image_projection_based_fusion::RoiPointCloudFusionNode) diff --git a/perception/image_projection_based_fusion/src/utils/utils.cpp b/perception/image_projection_based_fusion/src/utils/utils.cpp index 670c5596001fb..76cd1e3c61e82 100644 --- a/perception/image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/image_projection_based_fusion/src/utils/utils.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include "image_projection_based_fusion/utils/utils.hpp" - namespace image_projection_based_fusion { @@ -39,4 +38,197 @@ Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t) return a; } +void convertCluster2FeatureObject( + const std_msgs::msg::Header & header, const PointCloud & cluster, + DetectedObjectWithFeature & feature_obj) +{ + PointCloud2 ros_cluster; + pcl::toROSMsg(cluster, ros_cluster); + ros_cluster.header = header; + feature_obj.feature.cluster = ros_cluster; + feature_obj.object.kinematics.pose_with_covariance.pose.position = getCentroid(ros_cluster); + feature_obj.object.existence_probability = 1.0f; +} + +PointCloud closest_cluster( + PointCloud & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center) +{ + // sort point by distance to camera origin + + auto func = [center](const pcl::PointXYZ & p1, const pcl::PointXYZ & p2) { + return tier4_autoware_utils::calcDistance2d(center, p1) < + tier4_autoware_utils::calcDistance2d(center, p2); + }; + std::sort(cluster.begin(), cluster.end(), func); + PointCloud out_cluster; + for (auto & point : cluster) { + if (out_cluster.empty()) { + out_cluster.push_back(point); + continue; + } + if (tier4_autoware_utils::calcDistance2d(out_cluster.back(), point) < cluster_2d_tolerance) { + out_cluster.push_back(point); + continue; + } + if (out_cluster.size() >= static_cast(min_cluster_size)) { + return out_cluster; + } + out_cluster.clear(); + out_cluster.push_back(point); + } + return out_cluster; +} + +void updateOutputFusedObjects( + std::vector & output_objs, const std::vector & clusters, + const std_msgs::msg::Header & in_cloud_header, const std_msgs::msg::Header & in_roi_header, + const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const float cluster_2d_tolerance, + std::vector & output_fused_objects) +{ + if (output_objs.size() != clusters.size()) { + return; + } + Eigen::Vector3d orig_camera_frame, orig_point_frame; + Eigen::Affine3d camera2lidar_affine; + orig_camera_frame << 0.0, 0.0, 0.0; + { + const auto transform_stamped_optional = getTransformStamped( + tf_buffer, in_cloud_header.frame_id, in_roi_header.frame_id, in_roi_header.stamp); + if (!transform_stamped_optional) { + return; + } + camera2lidar_affine = transformToEigen(transform_stamped_optional.value().transform); + } + orig_point_frame = camera2lidar_affine * orig_camera_frame; + pcl::PointXYZ camera_orig_point_frame = + pcl::PointXYZ(orig_point_frame.x(), orig_point_frame.y(), orig_point_frame.z()); + + for (std::size_t i = 0; i < output_objs.size(); ++i) { + PointCloud cluster = clusters.at(i); + auto & feature_obj = output_objs.at(i); + if (cluster.points.size() < std::size_t(min_cluster_size)) { + continue; + } + + // TODO(badai-nguyen): change to interface to select refine criteria like closest, largest + // to output refine cluster and centroid + auto refine_cluster = + closest_cluster(cluster, cluster_2d_tolerance, min_cluster_size, camera_orig_point_frame); + if (refine_cluster.points.size() < std::size_t(min_cluster_size)) { + continue; + } + + refine_cluster.width = refine_cluster.points.size(); + refine_cluster.height = 1; + refine_cluster.is_dense = false; + // convert cluster to object + convertCluster2FeatureObject(in_cloud_header, refine_cluster, feature_obj); + output_fused_objects.push_back(feature_obj); + } +} + +void addShapeAndKinematic( + const pcl::PointCloud & cluster, + tier4_perception_msgs::msg::DetectedObjectWithFeature & feature_obj) +{ + if (cluster.empty()) { + return; + } + pcl::PointXYZ centroid = pcl::PointXYZ(0.0, 0.0, 0.0); + float max_z = -1e6; + float min_z = 1e6; + for (const auto & point : cluster) { + centroid.x += point.x; + centroid.y += point.y; + centroid.z += point.z; + max_z = max_z < point.z ? point.z : max_z; + min_z = min_z > point.z ? point.z : min_z; + } + centroid.x = centroid.x / static_cast(cluster.size()); + centroid.y = centroid.y / static_cast(cluster.size()); + centroid.z = centroid.z / static_cast(cluster.size()); + + std::vector cluster2d; + std::vector cluster2d_convex; + + for (size_t i = 0; i < cluster.size(); ++i) { + cluster2d.push_back( + cv::Point((cluster.at(i).x - centroid.x) * 1000.0, (cluster.at(i).y - centroid.y) * 1000.)); + } + cv::convexHull(cluster2d, cluster2d_convex); + if (cluster2d_convex.empty()) { + return; + } + pcl::PointXYZ polygon_centroid = pcl::PointXYZ(0.0, 0.0, 0.0); + for (size_t i = 0; i < cluster2d_convex.size(); ++i) { + polygon_centroid.x += static_cast(cluster2d_convex.at(i).x) / 1000.0; + polygon_centroid.y += static_cast(cluster2d_convex.at(i).y) / 1000.0; + } + polygon_centroid.x = polygon_centroid.x / static_cast(cluster2d_convex.size()); + polygon_centroid.y = polygon_centroid.y / static_cast(cluster2d_convex.size()); + + autoware_auto_perception_msgs::msg::Shape shape; + for (size_t i = 0; i < cluster2d_convex.size(); ++i) { + geometry_msgs::msg::Point32 point; + point.x = cluster2d_convex.at(i).x / 1000.0; + point.y = cluster2d_convex.at(i).y / 1000.0; + point.z = 0.0; + shape.footprint.points.push_back(point); + } + shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + constexpr float eps = 0.01; + shape.dimensions.x = 0; + shape.dimensions.y = 0; + shape.dimensions.z = std::max((max_z - min_z), eps); + feature_obj.object.shape = shape; + feature_obj.object.kinematics.pose_with_covariance.pose.position.x = + centroid.x + polygon_centroid.x; + feature_obj.object.kinematics.pose_with_covariance.pose.position.y = + centroid.y + polygon_centroid.y; + feature_obj.object.kinematics.pose_with_covariance.pose.position.z = + min_z + shape.dimensions.z * 0.5; + feature_obj.object.existence_probability = 1.0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.x = 0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.y = 0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.z = 0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.w = 1; +} + +geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud) +{ + geometry_msgs::msg::Point centroid; + centroid.x = 0.0f; + centroid.y = 0.0f; + centroid.z = 0.0f; + for (sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"), + iter_y(pointcloud, "y"), iter_z(pointcloud, "z"); + iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { + centroid.x += *iter_x; + centroid.y += *iter_y; + centroid.z += *iter_z; + } + const size_t size = pointcloud.width * pointcloud.height; + centroid.x = centroid.x / static_cast(size); + centroid.y = centroid.y / static_cast(size); + centroid.z = centroid.z / static_cast(size); + return centroid; +} + +pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster) +{ + pcl::PointXYZ closest_point; + double min_dist = 1e6; + pcl::PointXYZ orig_point = pcl::PointXYZ(0.0, 0.0, 0.0); + for (std::size_t i = 0; i < cluster.points.size(); ++i) { + pcl::PointXYZ point = cluster.points.at(i); + double dist_closest_point = tier4_autoware_utils::calcDistance2d(point, orig_point); + if (min_dist > dist_closest_point) { + min_dist = dist_closest_point; + closest_point = pcl::PointXYZ(point.x, point.y, point.z); + } + } + return closest_point; +} + } // namespace image_projection_based_fusion diff --git a/perception/lidar_apollo_segmentation_tvm/package.xml b/perception/lidar_apollo_segmentation_tvm/package.xml index f4f283abe9450..21cd79c27b205 100755 --- a/perception/lidar_apollo_segmentation_tvm/package.xml +++ b/perception/lidar_apollo_segmentation_tvm/package.xml @@ -21,6 +21,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros + tier4_autoware_utils tier4_perception_msgs tvm_utility tvm_vendor diff --git a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp index fb7ad38d7d642..905d70235e33e 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz index 9a390d10895f8..2fc71be9d34dc 100644 --- a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz +++ b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz @@ -1233,7 +1233,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/startplanner + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/start_planner Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz index 8ec93bb5b50d6..4a4c42f2b5e8e 100644 --- a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz +++ b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz @@ -1234,7 +1234,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/startplanner + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/start_planner Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false diff --git a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp index 9b01ea8f53675..53e19500ff428 100644 --- a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp @@ -189,7 +189,7 @@ void CenterPointTRT::postProcess(std::vector & det_boxes3d) head_out_heatmap_d_.get(), head_out_offset_d_.get(), head_out_z_d_.get(), head_out_dim_d_.get(), head_out_rot_d_.get(), head_out_vel_d_.get(), det_boxes3d, stream_)); if (det_boxes3d.size() == 0) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("lidar_centerpoint"), "No detected boxes."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lidar_centerpoint"), "No detected boxes."); } } diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index d487bb163c5e8..7631b47772862 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -151,23 +151,21 @@ If the target object is inside the road or crosswalk, this module outputs one or ## Parameters -| Parameter | Type | Description | -| ------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------ | -| `enable_delay_compensation` | bool | flag to enable the time delay compensation for the position of the object | -| `prediction_time_horizon` | double | predict time duration for predicted path [s] | -| `prediction_sampling_delta_time` | double | sampling time for points in predicted path [s] | -| `min_velocity_for_map_based_prediction` | double | apply map-based prediction to the objects with higher velocity than this value | -| `min_crosswalk_user_velocity` | double | minimum velocity use in path prediction for crosswalk users | -| `dist_threshold_for_searching_lanelet` | double | The threshold of the angle used when searching for the lane to which the object belongs [rad] | -| `delta_yaw_threshold_for_searching_lanelet` | double | The threshold of the distance used when searching for the lane to which the object belongs [m] | -| `sigma_lateral_offset` | double | Standard deviation for lateral position of objects [m] | -| `sigma_yaw_angle` | double | Standard deviation yaw angle of objects [rad] | -| `object_buffer_time_length` | double | Time span of object history to store the information [s] | -| `history_time_length` | double | Time span of object information used for prediction [s] | -| `dist_ratio_threshold_to_left_bound` | double | Conditions for using lane change detection of objects. Distance to the left bound of lanelet. | -| `dist_ratio_threshold_to_right_bound` | double | Conditions for using lane change detection of objects. Distance to the right bound of lanelet. | -| `diff_dist_threshold_to_left_bound` | double | Conditions for using lane change detection of objects. Differential value of horizontal position of objects. | -| `diff_dist_threshold_to_right_bound` | double | Conditions for using lane change detection of objects. Differential value of horizontal position of objects. | +| Parameter | Unit | Type | Description | +| ---------------------------------------------------------------- | ----- | ------ | ------------------------------------------------------------------------------------------------------------------------------------- | +| `enable_delay_compensation` | [-] | bool | flag to enable the time delay compensation for the position of the object | +| `prediction_time_horizon` | [s] | double | predict time duration for predicted path | +| `prediction_sampling_delta_time` | [s] | double | sampling time for points in predicted path | +| `min_velocity_for_map_based_prediction` | [m/s] | double | apply map-based prediction to the objects with higher velocity than this value | +| `min_crosswalk_user_velocity` | [m/s] | double | minimum velocity used when crosswalk user's velocity is calculated | +| `max_crosswalk_user_delta_yaw_threshold_for_lanelet` | [rad] | double | maximum yaw difference between crosswalk user and lanelet to use in path prediction for crosswalk users | +| `dist_threshold_for_searching_lanelet` | [m] | double | The threshold of the angle used when searching for the lane to which the object belongs | +| `delta_yaw_threshold_for_searching_lanelet` | [rad] | double | The threshold of the angle used when searching for the lane to which the object belongs | +| `sigma_lateral_offset` | [m] | double | Standard deviation for lateral position of objects | +| `sigma_yaw_angle_deg` | [deg] | double | Standard deviation yaw angle of objects | +| `object_buffer_time_length` | [s] | double | Time span of object history to store the information | +| `history_time_length` | [s] | double | Time span of object information used for prediction | +| `prediction_time_horizon_rate_for_validate_shoulder_lane_length` | [-] | double | prediction path will disabled when the estimated path length exceeds lanelet length. This parameter control the estimated path length | ## Assumptions / Known limits diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 7364e3d11b650..8ef6d628aa2f6 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -17,14 +17,9 @@ #include "map_based_prediction/path_generator.hpp" -#include -#include -#include -#include #include #include #include -#include #include #include @@ -35,12 +30,9 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include #include #include diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml index 9b7b32e686cfb..d7ff85adc9193 100644 --- a/perception/map_based_prediction/package.xml +++ b/perception/map_based_prediction/package.xml @@ -10,6 +10,7 @@ Yoshi Ri Takayuki Murooka Kyoichi Sugahara + Kotaro Uetake Apache License 2.0 ament_cmake diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 3b4e010d0a855..0ef1443fde519 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -15,14 +15,24 @@ #include "map_based_prediction/map_based_prediction_node.hpp" #include +#include +#include +#include #include -#include +#include +#include +#include +#include #include #include #include +#include +#include +#include +#include #include #ifdef ROS_DISTRO_GALACTIC diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index b1f6319801d4a..547132410badf 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -15,8 +15,8 @@ #include "map_based_prediction/path_generator.hpp" #include -#include -#include +#include +#include #include diff --git a/perception/multi_object_tracker/models.md b/perception/multi_object_tracker/models.md index 8ca12e3191dc5..c83038ffa7b59 100644 --- a/perception/multi_object_tracker/models.md +++ b/perception/multi_object_tracker/models.md @@ -2,6 +2,8 @@ ## Tracking model + + ### CTRV model [1] CTRV model is a model that assumes constant turn rate and velocity magnitude. @@ -106,5 +108,10 @@ In other words, the offset value must be adjusted so that the input BBOX and the ## References -[1] Schubert, Robin & Richter, Eric & Wanielik, Gerd. (2008). Comparison and evaluation of advanced motion models for vehicle tracking. 1 - 6. 10.1109/ICIF.2008.4632283. + + +[1] Schubert, Robin & Richter, Eric & Wanielik, Gerd. (2008). Comparison and evaluation of advanced motion models for vehicle tracking. 1 - 6. 10.1109/ICIF.2008.4632283. + + + [2] Kong, Jason & Pfeiffer, Mark & Schildbach, Georg & Borrelli, Francesco. (2015). Kinematic and dynamic vehicle models for autonomous driving control design. 1094-1099. 10.1109/IVS.2015.7225830. diff --git a/perception/object_merger/include/object_association_merger/node.hpp b/perception/object_merger/include/object_association_merger/node.hpp index 1e5b9fad9c9ca..6815b59894083 100644 --- a/perception/object_merger/include/object_association_merger/node.hpp +++ b/perception/object_merger/include/object_association_merger/node.hpp @@ -59,14 +59,16 @@ class ObjectAssociationMergerNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_; rclcpp::Publisher::SharedPtr merged_object_pub_; - message_filters::Subscriber object0_sub_; - message_filters::Subscriber object1_sub_; - typedef message_filters::sync_policies::ApproximateTime< + message_filters::Subscriber object0_sub_{}; + message_filters::Subscriber object1_sub_{}; + + using SyncPolicy = message_filters::sync_policies::ApproximateTime< autoware_auto_perception_msgs::msg::DetectedObjects, - autoware_auto_perception_msgs::msg::DetectedObjects> - SyncPolicy; - typedef message_filters::Synchronizer Sync; - Sync sync_; + autoware_auto_perception_msgs::msg::DetectedObjects>; + using Sync = message_filters::Synchronizer; + typename std::shared_ptr sync_ptr_; + + int sync_queue_size_; std::unique_ptr data_association_; std::string base_link_frame_id_; // associated with the base_link frame diff --git a/perception/object_merger/launch/object_association_merger.launch.xml b/perception/object_merger/launch/object_association_merger.launch.xml index 3418f7d5a5e61..a21ae8f7fa327 100644 --- a/perception/object_merger/launch/object_association_merger.launch.xml +++ b/perception/object_merger/launch/object_association_merger.launch.xml @@ -6,6 +6,7 @@ + @@ -14,6 +15,7 @@ + diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index 0ad2a76f74754..f776d2d940045 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -77,20 +77,13 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio tf_buffer_(get_clock()), tf_listener_(tf_buffer_), object0_sub_(this, "input/object0", rclcpp::QoS{1}.get_rmw_qos_profile()), - object1_sub_(this, "input/object1", rclcpp::QoS{1}.get_rmw_qos_profile()), - sync_(SyncPolicy(10), object0_sub_, object1_sub_) + object1_sub_(this, "input/object1", rclcpp::QoS{1}.get_rmw_qos_profile()) { - // Create publishers and subscribers - using std::placeholders::_1; - using std::placeholders::_2; - sync_.registerCallback(std::bind(&ObjectAssociationMergerNode::objectsCallback, this, _1, _2)); - merged_object_pub_ = create_publisher( - "output/object", rclcpp::QoS{1}); - // Parameters base_link_frame_id_ = declare_parameter("base_link_frame_id", "base_link"); priority_mode_ = static_cast( declare_parameter("priority_mode", static_cast(PriorityMode::Confidence))); + sync_queue_size_ = declare_parameter("sync_queue_size", 20); remove_overlapped_unknown_objects_ = declare_parameter("remove_overlapped_unknown_objects", true); overlapped_judge_param_.precision_threshold = @@ -115,6 +108,16 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio const auto min_iou_matrix = this->declare_parameter>("min_iou_matrix"); data_association_ = std::make_unique( can_assign_matrix, max_dist_matrix, max_rad_matrix, min_iou_matrix); + + // Create publishers and subscribers + using std::placeholders::_1; + using std::placeholders::_2; + sync_ptr_ = std::make_shared(SyncPolicy(sync_queue_size_), object0_sub_, object1_sub_); + sync_ptr_->registerCallback( + std::bind(&ObjectAssociationMergerNode::objectsCallback, this, _1, _2)); + + merged_object_pub_ = create_publisher( + "output/object", rclcpp::QoS{1}); } void ObjectAssociationMergerNode::objectsCallback( diff --git a/perception/radar_object_tracker/README.md b/perception/radar_object_tracker/README.md index 9f11d50ce2b3f..739a4a745ff6c 100644 --- a/perception/radar_object_tracker/README.md +++ b/perception/radar_object_tracker/README.md @@ -26,9 +26,10 @@ See more details in the [models.md](models.md). ### Input -| Name | Type | Description | -| --------- | ----------------------------------------------------- | ---------------- | -| `~/input` | `autoware_auto_perception_msgs::msg::DetectedObjects` | Detected objects | +| Name | Type | Description | +| ------------- | ----------------------------------------------------- | ---------------- | +| `~/input` | `autoware_auto_perception_msgs::msg::DetectedObjects` | Detected objects | +| `/vector/map` | `autoware_auto_msgs::msg::HADMapBin` | Map data | ### Output @@ -40,20 +41,29 @@ See more details in the [models.md](models.md). ### Node Parameters -| Name | Type | Default Value | Description | -| --------------------------- | ------ | ----------------------------- | ----------------------------------------------------------- | -| `publish_rate` | double | 30.0 | The rate at which to publish the output messages | -| `world_frame_id` | string | "world" | The frame ID of the world coordinate system | -| `enable_delay_compensation` | bool | false | Whether to enable delay compensation | -| `tracking_config_directory` | string | "" | The directory containing the tracking configuration files | -| `enable_logging` | bool | false | Whether to enable logging | -| `logging_file_path` | string | "~/.ros/association_log.json" | The path to the file where logs should be written | -| `can_assign_matrix` | array | | An array of integers used in the data association algorithm | -| `max_dist_matrix` | array | | An array of doubles used in the data association algorithm | -| `max_area_matrix` | array | | An array of doubles used in the data association algorithm | -| `min_area_matrix` | array | | An array of doubles used in the data association algorithm | -| `max_rad_matrix` | array | | An array of doubles used in the data association algorithm | -| `min_iou_matrix` | array | | An array of doubles used in the data association algorithm | +| Name | Type | Default Value | Description | +| ------------------------------------ | ------ | --------------------------- | --------------------------------------------------------------------------------------------------------------- | +| `publish_rate` | double | 10.0 | The rate at which to publish the output messages | +| `world_frame_id` | string | "map" | The frame ID of the world coordinate system | +| `enable_delay_compensation` | bool | false | Whether to enable delay compensation. If set to `true`, output topic is published by timer with `publish_rate`. | +| `tracking_config_directory` | string | "" | The directory containing the tracking configuration files | +| `enable_logging` | bool | false | Whether to enable logging | +| `logging_file_path` | string | "/tmp/association_log.json" | The path to the file where logs should be written | +| `tracker_lifetime` | double | 1.0 | The lifetime of the tracker in seconds | +| `use_distance_based_noise_filtering` | bool | true | Whether to use distance based filtering | +| `minimum_range_threshold` | double | 70.0 | Minimum distance threshold for filtering in meters | +| `use_map_based_noise_filtering` | bool | true | Whether to use map based filtering | +| `max_distance_from_lane` | double | 5.0 | Maximum distance from lane for filtering in meters | +| `max_angle_diff_from_lane` | double | 0.785398 | Maximum angle difference from lane for filtering in radians | +| `max_lateral_velocity` | double | 5.0 | Maximum lateral velocity for filtering in m/s | +| `can_assign_matrix` | array | | An array of integers used in the data association algorithm | +| `max_dist_matrix` | array | | An array of doubles used in the data association algorithm | +| `max_area_matrix` | array | | An array of doubles used in the data association algorithm | +| `min_area_matrix` | array | | An array of doubles used in the data association algorithm | +| `max_rad_matrix` | array | | An array of doubles used in the data association algorithm | +| `min_iou_matrix` | array | | An array of doubles used in the data association algorithm | + +See more details in the [models.md](models.md). ## Assumptions / Known limits diff --git a/perception/radar_object_tracker/config/radar_object_tracker.param.yaml b/perception/radar_object_tracker/config/radar_object_tracker.param.yaml new file mode 100644 index 0000000000000..f80adffb41090 --- /dev/null +++ b/perception/radar_object_tracker/config/radar_object_tracker.param.yaml @@ -0,0 +1,27 @@ +# general parameters for radar_object_tracker node +/**: + ros__parameters: + # basic settings + world_frame_id: "map" + tracker_lifetime: 1.0 # [sec] + # if empty, use default config declared in this package + tracking_config_directory: "" + + # delay compensate parameters + publish_rate: 10.0 + enable_delay_compensation: false + + # logging + enable_logging: false + logging_file_path: "/tmp/association_log.json" + + # filtering + ## 1. distance based filtering: remove closer objects than this threshold + use_distance_based_noise_filtering: true + minimum_range_threshold: 70.0 # [m] + + ## 2. lanelet map based filtering + use_map_based_noise_filtering: true + max_distance_from_lane: 5.0 # [m] + max_angle_diff_from_lane: 0.785398 # [rad] (45 deg) + max_lateral_velocity: 5.0 # [m/s] diff --git a/perception/radar_object_tracker/image/noise_filtering.drawio.svg b/perception/radar_object_tracker/image/noise_filtering.drawio.svg new file mode 100644 index 0000000000000..cd044cdfa1379 --- /dev/null +++ b/perception/radar_object_tracker/image/noise_filtering.drawio.svg @@ -0,0 +1,165 @@ + + + + + + + + + + + +
+
+
self vehicle
+
+
+ + self vehicle + + + + + + + +
+
+
removed
+
+
+ + removed + + + + + + + +
+
+
distance threshold
+
+
+ + distance threshold + + + + + + + +
+
+
+ filtered object +
+
+
+ + filtered object + + + + + + + + + + + +
+
+
removed
+
+
+ + removed + + + + + + + + + +
+
+
+ lane distance +
+ threshold +
+
+
+ + lane distance... + + + + + + + + + + + +
+
+
+ removed +
+ with +
+ angle threshold +
+
+
+ + removed... + + + + + + + + + +
+
+
+ removed by +
+ high +
+ lateral velocity +
+
+
+ + removed by... + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp b/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp index 898a7855aabae..415ff24b34cc3 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp +++ b/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp @@ -33,6 +33,16 @@ #include #endif +#include +#include +#include + +#include +#include +#include +#include +#include +#include #include #include @@ -41,8 +51,11 @@ #include #include +using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware_auto_perception_msgs::msg::DetectedObject; using autoware_auto_perception_msgs::msg::DetectedObjects; +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; class RadarObjectTrackerNode : public rclcpp::Node { @@ -50,20 +63,23 @@ class RadarObjectTrackerNode : public rclcpp::Node explicit RadarObjectTrackerNode(const rclcpp::NodeOptions & node_options); private: + // pub-sub rclcpp::Publisher::SharedPtr tracked_objects_pub_; rclcpp::Subscription::SharedPtr detected_object_sub_; - rclcpp::TimerBase::SharedPtr publish_timer_; // publish timer + rclcpp::TimerBase::SharedPtr publish_timer_; // publish timer + rclcpp::Subscription::SharedPtr sub_map_; // map subscriber tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - + float tracker_lifetime_; std::map tracker_map_; void onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); void onTimer(); + void onMap(const HADMapBin::ConstSharedPtr map_msg); std::string world_frame_id_; // tracking frame std::string tracker_config_directory_; @@ -77,11 +93,36 @@ class RadarObjectTrackerNode : public rclcpp::Node std::string path; } logging_; + // noise reduction + bool use_distance_based_noise_filtering_; + bool use_map_based_noise_filtering_; + + // distance based noise reduction + double minimum_range_threshold_; + std::string sensor_frame_ = "base_link"; + + // map based noise reduction + bool map_is_loaded_ = false; + double max_distance_from_lane_; + double max_lateral_velocity_; + double max_angle_diff_from_lane_; + // Lanelet Map Pointers + std::shared_ptr lanelet_map_ptr_; + std::shared_ptr routing_graph_ptr_; + std::shared_ptr traffic_rules_ptr_; + // Crosswalk Entry Points + // lanelet::ConstLanelets crosswalks_; + void checkTrackerLifeCycle( std::list> & list_tracker, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform); void sanitizeTracker( std::list> & list_tracker, const rclcpp::Time & time); + void mapBasedNoiseFilter( + std::list> & list_tracker, const rclcpp::Time & time); + void distanceBasedNoiseFilter( + std::list> & list_tracker, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform); std::shared_ptr createNewTracker( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) const; diff --git a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml index 7ef3e58a64161..e2d30c1b69d19 100644 --- a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml +++ b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml @@ -7,16 +7,18 @@ + + + + + + - - - - diff --git a/perception/radar_object_tracker/models.md b/perception/radar_object_tracker/models.md index a4c53db156f0f..f4e4c6a02edaf 100644 --- a/perception/radar_object_tracker/models.md +++ b/perception/radar_object_tracker/models.md @@ -74,3 +74,33 @@ v_{k+1} &= v_k \\ \omega_{k+1} &= \omega_k \end{align} $$ + +## Noise filtering + +Radar sensors often have noisy measurement. So we use the following filter to reduce the false positive objects. + +The figure below shows the current noise filtering process. + +![noise_filter](image/noise_filtering.drawio.svg) + +### minimum range filter + +In most cases, Radar sensors are used with other sensors such as LiDAR and Camera, and Radar sensors are used to detect objects far away. So we can filter out objects that are too close to the sensor. + +`use_distance_based_noise_filtering` parameter is used to enable/disable this filter, and `minimum_range_threshold` parameter is used to set the threshold. + +### lanelet based filter + +With lanelet map information, We can filter out false positive objects that are not likely important obstacles. + +We filter out objects that satisfy the following conditions: + +- too large lateral distance from lane +- velocity direction is too different from lane direction +- too large lateral velocity + +Each condition can be set by the following parameters: + +- `max_distance_from_lane` +- `max_angle_diff_from_lane` +- `max_lateral_velocity` diff --git a/perception/radar_object_tracker/package.xml b/perception/radar_object_tracker/package.xml index 3ff6feb617baa..ad71e613c1a18 100644 --- a/perception/radar_object_tracker/package.xml +++ b/perception/radar_object_tracker/package.xml @@ -16,6 +16,7 @@ autoware_auto_perception_msgs eigen kalman_filter + lanelet2_extension mussp nlohmann-json-dev object_recognition_utils diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp index f32084493f34e..6b02836e1f933 100644 --- a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp +++ b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp @@ -60,6 +60,131 @@ boost::optional getTransformAnonymous( } } +// check if lanelet is close enough to the target lanelet +bool isDuplicated( + const std::pair & target_lanelet, + const lanelet::ConstLanelets & lanelets) +{ + const double CLOSE_LANELET_THRESHOLD = 0.1; + for (const auto & lanelet : lanelets) { + const auto target_lanelet_end_p = target_lanelet.second.centerline2d().back(); + const auto lanelet_end_p = lanelet.centerline2d().back(); + const double dist = std::hypot( + target_lanelet_end_p.x() - lanelet_end_p.x(), target_lanelet_end_p.y() - lanelet_end_p.y()); + if (dist < CLOSE_LANELET_THRESHOLD) { + return true; + } + } + + return false; +} + +// check if the lanelet is valid for object tracking +bool checkCloseLaneletCondition( + const std::pair & lanelet, const TrackedObject & object, + const double max_distance_from_lane, const double max_angle_diff_from_lane) +{ + // Step1. If we only have one point in the centerline, we will ignore the lanelet + if (lanelet.second.centerline().size() <= 1) { + return false; + } + + // Step2. Check if the obstacle is inside or close enough to the lanelet + lanelet::BasicPoint2d search_point( + object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y); + if (!lanelet::geometry::inside(lanelet.second, search_point)) { + const auto distance = lanelet.first; + if (distance > max_distance_from_lane) { + return false; + } + } + + // Step3. Calculate the angle difference between the lane angle and obstacle angle + const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double lane_yaw = lanelet::utils::getLaneletAngle( + lanelet.second, object.kinematics.pose_with_covariance.pose.position); + const double delta_yaw = object_yaw - lane_yaw; + const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double abs_norm_delta = std::fabs(normalized_delta_yaw); + + // Step4. Check if the closest lanelet is valid, and add all + // of the lanelets that are below max_dist and max_delta_yaw + const double object_vel = object.kinematics.twist_with_covariance.twist.linear.x; + const bool is_yaw_reversed = M_PI - max_angle_diff_from_lane < abs_norm_delta && object_vel < 0.0; + if (is_yaw_reversed || abs_norm_delta < max_angle_diff_from_lane) { + return true; + } + + return false; +} + +lanelet::ConstLanelets getClosestValidLanelets( + const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const double max_distance_from_lane, const double max_angle_diff_from_lane) +{ + // obstacle point + lanelet::BasicPoint2d search_point( + object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y); + + // nearest lanelet + std::vector> surrounding_lanelets = + lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, 10); + + // No Closest Lanelets + if (surrounding_lanelets.empty()) { + return {}; + } + + lanelet::ConstLanelets closest_lanelets; + for (const auto & lanelet : surrounding_lanelets) { + // Check if the close lanelets meet the necessary condition for start lanelets and + // Check if similar lanelet is inside the closest lanelet + if ( + !checkCloseLaneletCondition( + lanelet, object, max_distance_from_lane, max_angle_diff_from_lane) || + isDuplicated(lanelet, closest_lanelets)) { + continue; + } + + closest_lanelets.push_back(lanelet.second); + } + + return closest_lanelets; +} + +bool hasValidVelocityDirectionToLanelet( + const TrackedObject & object, const lanelet::ConstLanelets & lanelets, + const double max_lateral_velocity) +{ + // get object velocity direction + const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double object_vel_x = object.kinematics.twist_with_covariance.twist.linear.x; + const double object_vel_y = object.kinematics.twist_with_covariance.twist.linear.y; + const double object_vel_yaw = std::atan2(object_vel_y, object_vel_x); // local frame + const double object_vel_yaw_global = + tier4_autoware_utils::normalizeRadian(object_yaw + object_vel_yaw); + const double object_vel = std::hypot(object_vel_x, object_vel_y); + + for (const auto & lanelet : lanelets) { + // get lanelet angle + const double lane_yaw = lanelet::utils::getLaneletAngle( + lanelet, object.kinematics.pose_with_covariance.pose.position); + const double delta_yaw = object_vel_yaw_global - lane_yaw; + const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + + // get lateral velocity + const double lane_vel = object_vel * std::sin(normalized_delta_yaw); + // std::cout << "lane_vel: " << lane_vel << " , delta yaw:" << normalized_delta_yaw << " , + // object_vel " << object_vel <("output", rclcpp::QoS{1}); + sub_map_ = this->create_subscription( + "/vector_map", rclcpp::QoS{1}.transient_local(), + std::bind(&RadarObjectTrackerNode::onMap, this, std::placeholders::_1)); // Parameters - double publish_rate = declare_parameter("publish_rate", 30.0); - world_frame_id_ = declare_parameter("world_frame_id", "world"); - bool enable_delay_compensation{declare_parameter("enable_delay_compensation", false)}; - tracker_config_directory_ = declare_parameter("tracking_config_directory", ""); - logging_.enable = declare_parameter("enable_logging", false); - logging_.path = - declare_parameter("logging_file_path", "~/.ros/association_log.json"); + tracker_lifetime_ = declare_parameter("tracker_lifetime"); + double publish_rate = declare_parameter("publish_rate"); + world_frame_id_ = declare_parameter("world_frame_id"); + bool enable_delay_compensation{declare_parameter("enable_delay_compensation")}; + tracker_config_directory_ = declare_parameter("tracking_config_directory"); + logging_.enable = declare_parameter("enable_logging"); + logging_.path = declare_parameter("logging_file_path"); + + // noise filter + use_distance_based_noise_filtering_ = + declare_parameter("use_distance_based_noise_filtering"); + use_map_based_noise_filtering_ = declare_parameter("use_map_based_noise_filtering"); + minimum_range_threshold_ = declare_parameter("minimum_range_threshold"); + max_distance_from_lane_ = declare_parameter("max_distance_from_lane"); + max_angle_diff_from_lane_ = declare_parameter("max_angle_diff_from_lane"); + max_lateral_velocity_ = declare_parameter("max_lateral_velocity"); // Load tracking config file if (tracker_config_directory_.empty()) { @@ -132,6 +269,17 @@ RadarObjectTrackerNode::RadarObjectTrackerNode(const rclcpp::NodeOptions & node_ std::make_pair(Label::MOTORCYCLE, this->declare_parameter("motorcycle_tracker"))); } +// load map information to node parameter +void RadarObjectTrackerNode::onMap(const HADMapBin::ConstSharedPtr msg) +{ + RCLCPP_INFO(get_logger(), "[Radar Object Tracker]: Start loading lanelet"); + lanelet_map_ptr_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); + RCLCPP_INFO(get_logger(), "[Radar Object Tracker]: Map is loaded"); + map_is_loaded_ = true; +} + void RadarObjectTrackerNode::onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg) { @@ -176,6 +324,14 @@ void RadarObjectTrackerNode::onMeasurement( /* life cycle check */ checkTrackerLifeCycle(list_tracker_, measurement_time, *self_transform); + /* map based noise filter */ + if (map_is_loaded_ && use_map_based_noise_filtering_) { + mapBasedNoiseFilter(list_tracker_, measurement_time); + } + /* distance based noise filter */ + if (use_distance_based_noise_filtering_) { + distanceBasedNoiseFilter(list_tracker_, measurement_time, *self_transform); + } /* sanitize trackers */ sanitizeTracker(list_tracker_, measurement_time); @@ -227,6 +383,16 @@ void RadarObjectTrackerNode::onTimer() /* life cycle check */ checkTrackerLifeCycle(list_tracker_, current_time, *self_transform); + + /* map based noise filter */ + if (map_is_loaded_ && use_map_based_noise_filtering_) { + mapBasedNoiseFilter(list_tracker_, current_time); + } + /* distance based noise filter */ + if (use_distance_based_noise_filtering_) { + distanceBasedNoiseFilter(list_tracker_, current_time, *self_transform); + } + /* sanitize trackers */ sanitizeTracker(list_tracker_, current_time); @@ -238,12 +404,9 @@ void RadarObjectTrackerNode::checkTrackerLifeCycle( std::list> & list_tracker, const rclcpp::Time & time, [[maybe_unused]] const geometry_msgs::msg::Transform & self_transform) { - /* params */ - constexpr float max_elapsed_time = 1.0; - /* delete tracker */ for (auto itr = list_tracker.begin(); itr != list_tracker.end(); ++itr) { - const bool is_old = max_elapsed_time < (*itr)->getElapsedTimeFromLastUpdate(time); + const bool is_old = tracker_lifetime_ < (*itr)->getElapsedTimeFromLastUpdate(time); if (is_old) { auto erase_itr = itr; --itr; @@ -252,6 +415,51 @@ void RadarObjectTrackerNode::checkTrackerLifeCycle( } } +// remove objects by lanelet information +void RadarObjectTrackerNode::mapBasedNoiseFilter( + std::list> & list_tracker, const rclcpp::Time & time) +{ + for (auto itr = list_tracker.begin(); itr != list_tracker.end(); ++itr) { + autoware_auto_perception_msgs::msg::TrackedObject object; + (*itr)->getTrackedObject(time, object); + const auto closest_lanelets = getClosestValidLanelets( + object, lanelet_map_ptr_, max_distance_from_lane_, max_angle_diff_from_lane_); + + // 1. If the object is not close to any lanelet, delete the tracker + const bool no_closest_lanelet = closest_lanelets.empty(); + // 2. If the object velocity direction is not close to the lanelet direction, delete the tracker + const bool is_velocity_direction_close_to_lanelet = + hasValidVelocityDirectionToLanelet(object, closest_lanelets, max_lateral_velocity_); + if (no_closest_lanelet || !is_velocity_direction_close_to_lanelet) { + // std::cout << "object removed due to map based noise filter" << " no close lanelet: " << + // no_closest_lanelet << " velocity direction flag:" << is_velocity_direction_close_to_lanelet + // << std::endl; + itr = list_tracker.erase(itr); + --itr; + } + } +} + +// remove objects by distance +void RadarObjectTrackerNode::distanceBasedNoiseFilter( + std::list> & list_tracker, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) +{ + // remove objects that are too close + for (auto itr = list_tracker.begin(); itr != list_tracker.end(); ++itr) { + autoware_auto_perception_msgs::msg::TrackedObject object; + (*itr)->getTrackedObject(time, object); + const double distance = std::hypot( + object.kinematics.pose_with_covariance.pose.position.x - self_transform.translation.x, + object.kinematics.pose_with_covariance.pose.position.y - self_transform.translation.y); + if (distance < minimum_range_threshold_) { + // std::cout << "object removed due to small distance. distance: " << distance << std::endl; + itr = list_tracker.erase(itr); + --itr; + } + } +} + void RadarObjectTrackerNode::sanitizeTracker( std::list> & list_tracker, const rclcpp::Time & time) { diff --git a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp index 73a96da454833..693fccb7e937c 100644 --- a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp +++ b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp @@ -170,7 +170,7 @@ void SimpleObjectMergerNode::onTimer() output_objects.header.frame_id = node_param_.new_frame_id; for (size_t i = 0; i < input_topic_size; i++) { - double time_diff = (this->get_clock()->now()).seconds() - + double time_diff = rclcpp::Time(objects_data_.at(i)->header.stamp).seconds() - rclcpp::Time(objects_data_.at(0)->header.stamp).seconds(); if (std::abs(time_diff) < node_param_.timeout_threshold) { transform_ = transform_listener_->getTransform( diff --git a/perception/tensorrt_yolox/CMakeLists.txt b/perception/tensorrt_yolox/CMakeLists.txt index 01ade9bac43d6..80a7f0964b4bc 100644 --- a/perception/tensorrt_yolox/CMakeLists.txt +++ b/perception/tensorrt_yolox/CMakeLists.txt @@ -30,7 +30,7 @@ set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") if(NOT EXISTS "${DATA_PATH}") execute_process(COMMAND mkdir -p ${DATA_PATH}) endif() -function(download FILE_NAME FILE_HASH) +function(download SUB_DIR FILE_NAME FILE_HASH) message(STATUS "Checking and downloading ${FILE_NAME}") set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) set(STATUS_CODE 0) @@ -44,14 +44,14 @@ function(download FILE_NAME FILE_HASH) else() message(STATUS "diff ${FILE_NAME}") message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) + file(DOWNLOAD https://awf.ml.dev.web.auto/perception/${SUB_DIR}/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) list(GET DOWNLOAD_STATUS 0 STATUS_CODE) list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) endif() else() message(STATUS "not found ${FILE_NAME}") message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) + file(DOWNLOAD https://awf.ml.dev.web.auto/perception/${SUB_DIR}/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) list(GET DOWNLOAD_STATUS 0 STATUS_CODE) list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) endif() @@ -62,10 +62,13 @@ function(download FILE_NAME FILE_HASH) endif() endfunction() -download(yolox-tiny.onnx 97028baf73ce55e115599c9c60651b08) -download(yolox-sPlus-opt.onnx bf3b0155351f90fcdca2626acbfd3bcf) -download(yolox-sPlus-opt.EntropyV2-calibration.table c6e6f1999d5724a017516a956096701f) -download(label.txt 9ceadca8b72b6169ee6aabb861fe3e1e) +set(V1_PATH models/object_detection_yolox_s/v1) +download(models yolox-tiny.onnx 97028baf73ce55e115599c9c60651b08) +download(models yolox-sPlus-opt.onnx bf3b0155351f90fcdca2626acbfd3bcf) +download(models yolox-sPlus-opt.EntropyV2-calibration.table c6e6f1999d5724a017516a956096701f) +download(${V1_PATH} yolox-sPlus-T4-960x960-pseudo-finetune.onnx 37165a7e67bdaf6acff93925c628f2b2) +download(${V1_PATH} yolox-sPlus-T4-960x960-pseudo-finetune.EntropyV2-calibration.table d266ea9846b10e4dab53572152c6fd8f) +download(models label.txt 9ceadca8b72b6169ee6aabb861fe3e1e) ########## # tensorrt_yolox diff --git a/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml b/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml index 43f59e944f889..cb89f5829c65d 100644 --- a/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml +++ b/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml @@ -1,8 +1,9 @@ + - + diff --git a/perception/traffic_light_fine_detector/package.xml b/perception/traffic_light_fine_detector/package.xml index c54912e256c02..8ff9f2d133b52 100644 --- a/perception/traffic_light_fine_detector/package.xml +++ b/perception/traffic_light_fine_detector/package.xml @@ -4,7 +4,7 @@ traffic_light_fine_detector 0.1.0 The traffic_light_fine_detector package - Mingyu Li + Tao Zhong Shunsuke Miura Apache License 2.0 diff --git a/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp b/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp index 9db8d7242c64d..e46431a7b199e 100644 --- a/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp +++ b/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp @@ -16,7 +16,6 @@ #define TRAFFIC_LIGHT_MAP_BASED_DETECTOR__NODE_HPP_ #include -#include #include #include diff --git a/perception/traffic_light_map_based_detector/package.xml b/perception/traffic_light_map_based_detector/package.xml index 1021757e99a07..c2d53869fa3d5 100644 --- a/perception/traffic_light_map_based_detector/package.xml +++ b/perception/traffic_light_map_based_detector/package.xml @@ -22,6 +22,7 @@ sensor_msgs tf2 tf2_eigen + tf2_geometry_msgs tf2_ros tier4_autoware_utils tier4_perception_msgs diff --git a/perception/traffic_light_map_based_detector/src/node.cpp b/perception/traffic_light_map_based_detector/src/node.cpp index a46be77667bcb..f0a5d7b7b1fde 100644 --- a/perception/traffic_light_map_based_detector/src/node.cpp +++ b/perception/traffic_light_map_based_detector/src/node.cpp @@ -31,6 +31,12 @@ #include #include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + namespace { cv::Point2d calcRawImagePointFromPoint3D( diff --git a/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp b/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp index bdfd07ae54b28..390b0f590b637 100644 --- a/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp +++ b/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp @@ -15,8 +15,6 @@ #ifndef TRAFFIC_LIGHT_MULTI_CAMERA_FUSION__NODE_HPP_ #define TRAFFIC_LIGHT_MULTI_CAMERA_FUSION__NODE_HPP_ -#include -#include #include #include @@ -25,7 +23,7 @@ #include #include -#include +#include #include #include #include diff --git a/perception/traffic_light_multi_camera_fusion/package.xml b/perception/traffic_light_multi_camera_fusion/package.xml index eb0858152e69e..b08ccfe668085 100644 --- a/perception/traffic_light_multi_camera_fusion/package.xml +++ b/perception/traffic_light_multi_camera_fusion/package.xml @@ -4,7 +4,7 @@ traffic_light_multi_camera_fusion 0.1.0 The traffic_light_multi_camera_fusion package - Mingyu Li + Tao Zhong Shunsuke Miura Apache License 2.0 diff --git a/perception/traffic_light_multi_camera_fusion/src/node.cpp b/perception/traffic_light_multi_camera_fusion/src/node.cpp index 6c29be9858a9a..1582040648624 100644 --- a/perception/traffic_light_multi_camera_fusion/src/node.cpp +++ b/perception/traffic_light_multi_camera_fusion/src/node.cpp @@ -14,6 +14,9 @@ #include "traffic_light_multi_camera_fusion/node.hpp" +#include +#include + #include #include #include diff --git a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp index e37083f81decd..7e2825197f3a7 100644 --- a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp +++ b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp @@ -25,9 +25,7 @@ #include #include -#include -#include -#include +#include #include #include #include diff --git a/perception/traffic_light_occlusion_predictor/package.xml b/perception/traffic_light_occlusion_predictor/package.xml index 0ae390c90b9a9..49a67d68057d9 100644 --- a/perception/traffic_light_occlusion_predictor/package.xml +++ b/perception/traffic_light_occlusion_predictor/package.xml @@ -4,7 +4,7 @@ traffic_light_occlusion_predictor 0.1.0 The traffic_light_occlusion_predictor package - Mingyu Li + Tao Zhong Shunsuke Miura Apache License 2.0 diff --git a/perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp b/perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp index 07cb21e60b137..3f83cf6e926ad 100644 --- a/perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp +++ b/perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp @@ -16,7 +16,6 @@ #define TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ #include -#include #include #include diff --git a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp index a9d107e78a1c4..53e06e47a1873 100644 --- a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp +++ b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include diff --git a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp b/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp index a3b1a2b16a743..2f2d80ba43233 100644 --- a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp +++ b/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp @@ -127,13 +127,25 @@ void TrafficLightRoiVisualizerNodelet::imageRoiCallback( { cv_bridge::CvImagePtr cv_ptr; try { - cv_ptr = cv_bridge::toCvCopy(input_image_msg, input_image_msg->encoding); + // try to convert to RGB8 from any input encoding, since createRect() only supports RGB8 based + // bbox drawing + cv_ptr = cv_bridge::toCvCopy(input_image_msg, sensor_msgs::image_encodings::RGB8); for (auto tl_roi : input_tl_roi_msg->rois) { - createRect(cv_ptr->image, tl_roi, cv::Scalar(0, 255, 0)); + ClassificationResult result; + bool has_correspond_traffic_signal = + getClassificationResult(tl_roi.traffic_light_id, *input_traffic_signals_msg, result); + + if (!has_correspond_traffic_signal) { + // does not have classification result + createRect(cv_ptr->image, tl_roi, cv::Scalar(255, 255, 255)); + } else { + // has classification result + createRect(cv_ptr->image, tl_roi, result); + } } } catch (cv_bridge::Exception & e) { RCLCPP_ERROR( - get_logger(), "Could not convert from '%s' to 'bgr8'.", input_image_msg->encoding.c_str()); + get_logger(), "Could not convert from '%s' to 'rgb8'.", input_image_msg->encoding.c_str()); } image_pub_.publish(cv_ptr->toImageMsg()); } @@ -182,7 +194,9 @@ void TrafficLightRoiVisualizerNodelet::imageRoughRoiCallback( { cv_bridge::CvImagePtr cv_ptr; try { - cv_ptr = cv_bridge::toCvCopy(input_image_msg, input_image_msg->encoding); + // try to convert to RGB8 from any input encoding, since createRect() only supports RGB8 based + // bbox drawing + cv_ptr = cv_bridge::toCvCopy(input_image_msg, sensor_msgs::image_encodings::RGB8); for (auto tl_rough_roi : input_tl_rough_roi_msg->rois) { // visualize rough roi createRect(cv_ptr->image, tl_rough_roi, cv::Scalar(0, 255, 0)); @@ -208,7 +222,7 @@ void TrafficLightRoiVisualizerNodelet::imageRoughRoiCallback( } } catch (cv_bridge::Exception & e) { RCLCPP_ERROR( - get_logger(), "Could not convert from '%s' to 'bgr8'.", input_image_msg->encoding.c_str()); + get_logger(), "Could not convert from '%s' to 'rgb8'.", input_image_msg->encoding.c_str()); } image_pub_.publish(cv_ptr->toImageMsg()); } diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 39d05a7fb761b..709f271ac265c 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -2,7 +2,6 @@ ros__parameters: goal_planner: # general params - minimum_request_length: 100.0 th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 2.0 # It must be greater than the state_machine's. @@ -17,25 +16,28 @@ longitudinal_margin: 3.0 max_lateral_offset: 0.5 lateral_offset_interval: 0.25 - ignore_distance_from_lane_start: 15.0 + ignore_distance_from_lane_start: 0.0 margin_from_boundary: 0.5 # occupancy grid map occupancy_grid: - use_occupancy_grid: true - use_occupancy_grid_for_longitudinal_margin: false + use_occupancy_grid_for_goal_search: true + use_occupancy_grid_for_goal_longitudinal_margin: false + use_occupancy_grid_for_path_collision_check: false occupancy_grid_collision_check_margin: 0.0 theta_size: 360 obstacle_threshold: 60 # object recognition object_recognition: - use_object_recognition: true + use_object_recognition: false object_recognition_collision_check_margin: 0.6 # this should be larger than `surround_check_distance` of surround_obstacle_checker object_recognition_collision_check_max_extra_stopping_margin: 1.0 + th_moving_object_velocity: 1.0 # pull over pull_over: + minimum_request_length: 100.0 pull_over_velocity: 3.0 pull_over_minimum_velocity: 1.38 decide_path_distance: 10.0 @@ -104,6 +106,66 @@ neighbor_radius: 8.0 margin: 1.0 + stop_condition: + maximum_deceleration_for_stop: 1.0 + maximum_jerk_for_stop: 1.0 + path_safety_check: + # EgoPredictedPath + ego_predicted_path: + acceleration: 1.0 + time_horizon: 10.0 + time_resolution: 0.5 + min_slow_speed: 0.0 + delay_until_departure: 1.0 + target_velocity: 1.0 + # For target object filtering + target_filtering: + safety_check_time_horizon: 5.0 + safety_check_time_resolution: 1.0 + # detection range + object_check_forward_distance: 10.0 + object_check_backward_distance: 100.0 + ignore_object_velocity_threshold: 0.0 + # ObjectTypesToCheck + object_types_to_check: + check_car: true + check_truck: true + check_bus: true + check_trailer: true + check_bicycle: true + check_motorcycle: true + check_pedestrian: true + check_unknown: false + # ObjectLaneConfiguration + object_lane_configuration: + check_current_lane: true + check_right_side_lane: true + check_left_side_lane: true + check_shoulder_lane: true + check_other_lane: false + include_opposite_lane: false + invert_opposite_lane: false + check_all_predicted_path: true + use_all_predicted_path: true + use_predicted_path_outside_lanelet: false + + # For safety check + safety_check_params: + # safety check configuration + enable_safety_check: true + # collision check parameters + check_all_predicted_path: true + publish_debug_marker: false + rss_params: + rear_vehicle_reaction_time: 1.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 1.0 + longitudinal_velocity_delta_time: 1.0 + # temporary + backward_path_length: 30.0 + forward_path_length: 100.0 + # debug debug: print_debug_info: false diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index bea78664c65a3..b62262423fa72 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -72,3 +72,63 @@ max_planning_time: 150.0 neighbor_radius: 8.0 margin: 1.0 + + stop_condition: + maximum_deceleration_for_stop: 1.0 + maximum_jerk_for_stop: 1.0 + path_safety_check: + # EgoPredictedPath + ego_predicted_path: + acceleration: 1.0 + time_horizon: 10.0 + time_resolution: 0.5 + min_slow_speed: 0.0 + delay_until_departure: 1.0 + target_velocity: 1.0 + # For target object filtering + target_filtering: + safety_check_time_horizon: 5.0 + safety_check_time_resolution: 1.0 + # detection range + object_check_forward_distance: 10.0 + object_check_backward_distance: 100.0 + ignore_object_velocity_threshold: 0.0 + # ObjectTypesToCheck + object_types_to_check: + check_car: true + check_truck: true + check_bus: true + check_trailer: true + check_bicycle: true + check_motorcycle: true + check_pedestrian: true + check_unknown: false + # ObjectLaneConfiguration + object_lane_configuration: + check_current_lane: true + check_right_side_lane: true + check_left_side_lane: true + check_shoulder_lane: true + check_other_lane: false + include_opposite_lane: false + invert_opposite_lane: false + check_all_predicted_path: true + use_all_predicted_path: true + use_predicted_path_outside_lanelet: false + + # For safety check + safety_check_params: + # safety check configuration + enable_safety_check: true + # collision check parameters + check_all_predicted_path: true + publish_debug_marker: false + rss_params: + rear_vehicle_reaction_time: 1.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 1.0 + longitudinal_velocity_delta_time: 1.0 + # temporary + backward_path_length: 30.0 + forward_path_length: 100.0 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index 0a78787c559d7..18b77f3faacf0 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -86,8 +86,8 @@ Either one is activated when all conditions are met. ### fixed_goal_planner -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. - Route is set with `allow_goal_modification=false` by default. +- ego-vehicle is in the same lane as the goal. @@ -95,7 +95,7 @@ Either one is activated when all conditions are met. #### pull over on road lane -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`. - Route is set with `allow_goal_modification=true` . - We can set this option with [SetRoute](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/routing/srv/SetRoute.srv#L2) api service. - We support `2D Rough Goal Pose` with the key bind `r` in RViz, but in the future there will be a panel of tools to manipulate various Route API from RViz. @@ -105,7 +105,7 @@ Either one is activated when all conditions are met. #### pull over on shoulder lane -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`. - Goal is set in the `road_shoulder`. @@ -118,17 +118,11 @@ Either one is activated when all conditions are met. ## General parameters for goal_planner -| Name | Unit | Type | Description | Default value | -| :------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, the module is activated. | 100.0 | -| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | -| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | -| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | -| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | -| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | -| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | +| Name | Unit | Type | Description | Default value | +| :------------------ | :---- | :----- | :------------------------------------------------- | :------------ | +| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | ## **collision check** @@ -138,13 +132,14 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio #### Parameters for occupancy grid based collision check -| Name | Unit | Type | Description | Default value | -| :----------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | -| use_occupancy_grid | [-] | bool | flag whether to use occupancy grid for collision check | true | -| use_occupancy_grid_for_longitudinal_margin | [-] | bool | flag whether to use occupancy grid for keeping longitudinal margin | false | -| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 | -| theta_size | [-] | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | -| obstacle_threshold | [-] | int | threshold of cell values to be considered as obstacles | 60 | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | +| use_occupancy_grid_for_goal_search | [-] | bool | flag whether to use occupancy grid for goal search collision check | true | +| use_occupancy_grid_for_goal_longitudinal_margin | [-] | bool | flag whether to use occupancy grid for keeping longitudinal margin | false | +| use_occupancy_grid_for_path_collision_check | [-] | bool | flag whether to use occupancy grid for collision check | false | +| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 | +| theta_size | [-] | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | +| obstacle_threshold | [-] | int | threshold of cell values to be considered as obstacles | 60 | ### **object recognition based collision check** @@ -174,12 +169,22 @@ searched for in certain range of the shoulder lane. | longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | | max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | | lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 15.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | -## **Path Generation** +## **Pull Over** There are three path generation methods. -The path is generated with a certain margin (default: `0.5 m`) from left boundary of shoulder lane. +The path is generated with a certain margin (default: `0.5 m`) from the boundary of shoulder lane. + +| Name | Unit | Type | Description | Default value | +| :------------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| pull_over_minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, pull over is activated. | 100.0 | +| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | +| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | +| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | +| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | ### **shift parking** diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 9217e139d218c..78172c6fc0750 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include #include 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 ba8ddd9b19c46..c53735486efb0 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 @@ -17,9 +17,8 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" -#include +#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index b77ac16a9db61..4bdfc807aa7c4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_following/module_data.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 096ae61a9432e..a719ab341b88b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -325,10 +325,16 @@ class AvoidanceModule : public SceneModuleInterface * 3. merge raw shirt lines. * 4. trim unnecessary shift lines. */ - AvoidLineArray applyPreProcessToRawShiftLines( + AvoidLineArray applyPreProcess( AvoidLineArray & current_raw_shift_points, DebugData & debug) const; - AvoidLineArray getFillGapShiftLines(const AvoidLineArray & shift_lines) const; + /* + * @brief fill gap among shift lines. + * @param original shift lines. + * @param debug data. + * @return processed shift lines. + */ + AvoidLineArray applyFillGapProcess(const AvoidLineArray & shift_lines, DebugData & debug) const; /* * @brief merge negative & positive shift lines. @@ -336,7 +342,18 @@ class AvoidanceModule : public SceneModuleInterface * @param debug data. * @return processed shift lines. */ - AvoidLineArray mergeShiftLines(const AvoidLineArray & raw_shift_lines, DebugData & debug) const; + AvoidLineArray applyMergeProcess(const AvoidLineArray & shift_lines, DebugData & debug) const; + + /* + * @brief add return shift line from ego position. + * @param shift lines which the return shift is added. + * Pick up the last shift point, which is the most farthest from ego, from the current candidate + * avoidance points and registered points in the shifter. If the last shift length of the point is + * non-zero, add a return-shift to center line from the point. If there is no shift point in + * candidate avoidance points nor registered points, and base_shift > 0, add a return-shift to + * center line from ego. + */ + AvoidLineArray addReturnShiftLine(const AvoidLineArray & shift_lines, DebugData & debug) const; /* * @brief extract shift lines from total shift lines based on their gradient. @@ -356,7 +373,7 @@ class AvoidanceModule : public SceneModuleInterface * - Change the shift length to the previous one if the deviation is small. * - Remove unnecessary return shift (back to the center line). */ - AvoidLineArray trimShiftLine(const AvoidLineArray & shift_lines, DebugData & debug) const; + AvoidLineArray applyTrimProcess(const AvoidLineArray & shift_lines, DebugData & debug) const; /* * @brief extract new shift lines based on current shifted path. the module makes a RTC request @@ -366,17 +383,6 @@ class AvoidanceModule : public SceneModuleInterface */ AvoidLineArray findNewShiftLine(const AvoidLineArray & shift_lines) const; - /* - * @brief add return shift line from ego position. - * @param shift lines which the return shift is added. - * Pick up the last shift point, which is the most farthest from ego, from the current candidate - * avoidance points and registered points in the shifter. If the last shift length of the point is - * non-zero, add a return-shift to center line from the point. If there is no shift point in - * candidate avoidance points nor registered points, and base_shift > 0, add a return-shift to - * center line from ego. - */ - void addReturnShiftLineFromEgo(AvoidLineArray & shift_lines) const; - /* * @brief fill gap between two shift lines. * @param original shift lines. @@ -397,27 +403,27 @@ class AvoidanceModule : public SceneModuleInterface * @param threshold. shift length is quantized by this value. (if it is 0.3[m], the output shift * length is 0.0, 0.3, 0.6...) */ - void quantizeShiftLine(AvoidLineArray & shift_lines, const double threshold) const; + void applyQuantizeProcess(AvoidLineArray & shift_lines, const double threshold) const; /* * @brief trim shift line whose relative longitudinal distance is less than threshold. * @param target shift lines. * @param threshold. */ - void trimSmallShiftLine(AvoidLineArray & shift_lines, const double threshold) const; + void applySmallShiftFilter(AvoidLineArray & shift_lines, const double threshold) const; /* * @brief merge multiple shift lines whose relative gradient is less than threshold. * @param target shift lines. * @param threshold. */ - void trimSimilarGradShiftLine(AvoidLineArray & shift_lines, const double threshold) const; + void applySimilarGradFilter(AvoidLineArray & shift_lines, const double threshold) const; /* * @brief trim invalid shift lines whose gradient it too large to follow. * @param target shift lines. */ - void trimSharpReturn(AvoidLineArray & shift_lines, const double threshold) const; + void applySharpShiftFilter(AvoidLineArray & shift_lines, const double threshold) const; /** * @brief add new shift line to path shifter if the RTC status is activated. @@ -432,7 +438,8 @@ class AvoidanceModule : public SceneModuleInterface void addNewShiftLines(PathShifter & path_shifter, const AvoidLineArray & shift_lines) const; /** - * @brief validate shift lines. + * @brief once generate avoidance path from new shift lines, and calculate lateral offset between + * ego and the path. * @param new shift lines. * @param path shifter. * @return result. if there is huge gap between the ego position and candidate path, return false. 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 d7f824ba8f419..a6f9565c08aae 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 @@ -24,6 +24,9 @@ #include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp" #include "behavior_path_planner/utils/goal_planner/shift_pull_over.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" +#include "behavior_path_planner/utils/utils.hpp" #include #include @@ -57,6 +60,12 @@ using freespace_planning_algorithms::PlannerCommonParam; using freespace_planning_algorithms::RRTStar; using freespace_planning_algorithms::RRTStarParam; +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; + enum class PathType { NONE = 0, SHIFT, @@ -72,13 +81,16 @@ struct PullOverStatus size_t current_path_idx{0}; bool require_increment_{true}; // if false, keep current path idx. std::shared_ptr prev_stop_path{nullptr}; - lanelet::ConstLanelets current_lanes{}; - lanelet::ConstLanelets pull_over_lanes{}; - std::vector lanes{}; // current + pull_over - bool has_decided_path{false}; - bool is_safe_static_objects{false}; // current path is safe against static objects - bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects + std::shared_ptr prev_stop_path_after_approval{nullptr}; + // stop path after approval, stop path is not updated until safety is confirmed + lanelet::ConstLanelets current_lanes{}; // TODO(someone): explain + lanelet::ConstLanelets pull_over_lanes{}; // TODO(someone): explain + std::vector lanes{}; // current + pull_over + bool has_decided_path{false}; // if true, the path has is decided and safe against static objects + bool is_safe_static_objects{false}; // current path is safe against *static* objects + bool is_safe_dynamic_objects{false}; // current path is safe against *dynamic* objects bool prev_is_safe{false}; + bool prev_is_safe_dynamic_objects{false}; bool has_decided_velocity{false}; bool has_requested_approval{false}; bool is_ready{false}; @@ -107,6 +119,13 @@ class GoalPlannerModule : public SceneModuleInterface void updateModuleParams(const std::any & parameters) override { parameters_ = std::any_cast>(parameters); + if (parameters_->safety_check_params.enable_safety_check) { + ego_predicted_path_params_ = + std::make_shared(parameters_->ego_predicted_path_params); + objects_filtering_params_ = + std::make_shared(parameters_->objects_filtering_params); + safety_check_params_ = std::make_shared(parameters_->safety_check_params); + } } // TODO(someone): remove this, and use base class function @@ -137,8 +156,14 @@ class GoalPlannerModule : public SceneModuleInterface PullOverStatus status_; + mutable StartGoalPlannerData goal_planner_data_; + std::shared_ptr parameters_; + mutable std::shared_ptr ego_predicted_path_params_; + mutable std::shared_ptr objects_filtering_params_; + mutable std::shared_ptr safety_check_params_; + vehicle_info_util::VehicleInfo vehicle_info_; // planner @@ -208,6 +233,18 @@ class GoalPlannerModule : public SceneModuleInterface const Pose & search_start_offset_pose, PathWithLaneId & path) const; PathWithLaneId generateStopPath(); PathWithLaneId generateFeasibleStopPath(); + + /** + * @brief Generate a stop point in the current path based on the vehicle's pose and constraints. + * + * This function generates a stop point in the current path. If no lanes are available or if + * stopping is not feasible due to constraints (maximum deceleration, maximum jerk), no stop point + * is inserted into the path. + * + * @return the modified path with the stop point inserted. If no feasible stop point can be + * determined, returns an empty optional. + */ + std::optional generateStopInsertedCurrentPath(); void keepStoppedWithCurrentPath(PathWithLaneId & path); double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; @@ -255,6 +292,17 @@ class GoalPlannerModule : public SceneModuleInterface // output setter void setOutput(BehaviorModuleOutput & output); void setStopPath(BehaviorModuleOutput & output); + + /** + * @brief Sets a stop path in the current path based on safety conditions and previous paths. + * + * This function sets a stop path in the current path. Depending on whether the previous safety + * judgement against dynamic objects were safe or if a previous stop path existed, it either + * generates a new stop path or uses the previous stop path. + * + * @param output BehaviorModuleOutput + */ + void setStopPathFromCurrentPath(BehaviorModuleOutput & output); void setModifiedGoal(BehaviorModuleOutput & output) const; void setTurnSignalInfo(BehaviorModuleOutput & output) const; @@ -276,6 +324,14 @@ class GoalPlannerModule : public SceneModuleInterface // rtc std::pair calcDistanceToPathChange() const; + // safety check + void initializeSafetyCheckParameters(); + SafetyCheckParams createSafetyCheckParams() const; + void updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const; + bool isSafePath() const; + // debug void setDebugData(); void printParkingPositionError() const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index 6acf5b945308f..077f7a3a763d5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -22,10 +22,15 @@ #include #include #include +#include +#include +#include #include #include #include +#include #include +#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 2e8c9e57f8842..e9a94126d9f0e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -17,12 +17,15 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" #include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" +#include "behavior_path_planner/utils/utils.hpp" #include #include @@ -43,6 +46,11 @@ namespace behavior_path_planner { +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using geometry_msgs::msg::PoseArray; using lane_departure_checker::LaneDepartureChecker; @@ -55,7 +63,8 @@ struct PullOutStatus lanelet::ConstLanelets pull_out_lanes{}; bool is_safe_static_objects{false}; // current path is safe against static objects bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects - bool back_finished{false}; + bool back_finished{false}; // if backward driving is not required, this is also set to true + // todo: rename to clear variable name. Pose pull_out_start_pose{}; PullOutStatus() {} @@ -84,12 +93,19 @@ class StartPlannerModule : public SceneModuleInterface BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; CandidateOutput planCandidate() const override; - + void processOnEntry() override; void processOnExit() override; void setParameters(const std::shared_ptr & parameters) { parameters_ = parameters; + if (parameters->safety_check_params.enable_safety_check) { + ego_predicted_path_params_ = + std::make_shared(parameters_->ego_predicted_path_params); + objects_filtering_params_ = + std::make_shared(parameters_->objects_filtering_params); + safety_check_params_ = std::make_shared(parameters_->safety_check_params); + } } void resetStatus(); @@ -109,11 +125,17 @@ class StartPlannerModule : public SceneModuleInterface bool canTransitIdleToRunningState() override { return false; } + void initializeSafetyCheckParameters(); + std::shared_ptr parameters_; + mutable std::shared_ptr ego_predicted_path_params_; + mutable std::shared_ptr objects_filtering_params_; + mutable std::shared_ptr safety_check_params_; vehicle_info_util::VehicleInfo vehicle_info_; std::vector> start_planners_; PullOutStatus status_; + mutable StartGoalPlannerData start_planner_data_; std::deque odometry_buffer_; @@ -131,7 +153,8 @@ class StartPlannerModule : public SceneModuleInterface std::mutex mutex_; PathWithLaneId getFullPath() const; - std::vector searchPullOutStartPoses(); + PathWithLaneId calcStartPoseCandidatesBackwardPath() const; + std::vector searchPullOutStartPoses(const PathWithLaneId & start_pose_candidates) const; std::shared_ptr lane_departure_checker_; @@ -141,8 +164,8 @@ class StartPlannerModule : public SceneModuleInterface void incrementPathIndex(); PathWithLaneId getCurrentPath() const; void planWithPriority( - const std::vector & start_pose_candidates, const Pose & goal_pose, - const std::string search_priority); + const std::vector & start_pose_candidates, const Pose & refined_start_pose, + const Pose & goal_pose, const std::string search_priority); PathWithLaneId generateStopPath() const; lanelet::ConstLanelets getPathRoadLanes(const PathWithLaneId & path) const; std::vector generateDrivableLanes(const PathWithLaneId & path) const; @@ -155,6 +178,10 @@ class StartPlannerModule : public SceneModuleInterface bool isStopped(); bool isStuck(); bool hasFinishedCurrentPath(); + void updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const; + bool isSafePath() const; void setDrivableAreaInfo(BehaviorModuleOutput & output) const; // check if the goal is located behind the ego in the same route segment. @@ -163,6 +190,7 @@ class StartPlannerModule : public SceneModuleInterface // generate BehaviorPathOutput with stopping path and update status BehaviorModuleOutput generateStopOutput(); + SafetyCheckParams createSafetyCheckParams() const; // freespace planner void onFreespacePlannerTimer(); bool planFreespacePath(); 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 3264ec1e9ddfd..9a47a6728ddb7 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 @@ -20,10 +20,11 @@ #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include -#include +#include #include #include +#include #include #include @@ -524,22 +525,29 @@ struct DebugData lanelet::ConstLineStrings3d bounds; - AvoidLineArray current_shift_lines; // in path shifter - AvoidLineArray new_shift_lines; // in path shifter - - AvoidLineArray registered_raw_shift; - AvoidLineArray current_raw_shift; - AvoidLineArray extra_return_shift; - - AvoidLineArray merged; - AvoidLineArray gap_filled; - AvoidLineArray trim_similar_grad_shift; - AvoidLineArray quantized; - AvoidLineArray trim_small_shift; - AvoidLineArray trim_similar_grad_shift_second; - AvoidLineArray trim_similar_grad_shift_third; - AvoidLineArray trim_momentary_return; - AvoidLineArray trim_too_sharp_shift; + // combine process + AvoidLineArray step1_registered_shift_line; + AvoidLineArray step1_current_raw_shift_line; + AvoidLineArray step1_filled_shift_line; + AvoidLineArray step1_merged_shift_line; + AvoidLineArray step1_combined_shift_line; + AvoidLineArray step1_return_shift_line; + AvoidLineArray step1_front_shift_line; + AvoidLineArray step1_shift_line; + + // create outline process + AvoidLineArray step2_merged_shift_line; + + // trimming process + AvoidLineArray step3_quantized_shift_line; + AvoidLineArray step3_noise_removed; + AvoidLineArray step3_grad_filtered_first; + AvoidLineArray step3_grad_filtered_second; + AvoidLineArray step3_grad_filtered_third; + + // registered process + AvoidLineArray step4_new_shift_line; + AvoidLineArray step4_valid_shift_line; // shift length std::vector pos_shift; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index 6b8a62d2caec9..4e40267ede4df 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -41,7 +41,6 @@ enum class ParkingPolicy { struct GoalPlannerParameters { // general params - double minimum_request_length; double th_arrived_distance; double th_stopped_velocity; double th_stopped_time; @@ -60,8 +59,9 @@ struct GoalPlannerParameters double margin_from_boundary; // occupancy grid map - bool use_occupancy_grid; - bool use_occupancy_grid_for_longitudinal_margin; + bool use_occupancy_grid_for_goal_search; + bool use_occupancy_grid_for_goal_longitudinal_margin; + bool use_occupancy_grid_for_path_collision_check; double occupancy_grid_collision_check_margin; int theta_size; int obstacle_threshold; @@ -70,8 +70,10 @@ struct GoalPlannerParameters bool use_object_recognition; double object_recognition_collision_check_margin; double object_recognition_collision_check_max_extra_stopping_margin; + double th_moving_object_velocity; // pull over general params + double pull_over_minimum_request_length; double pull_over_velocity; double pull_over_minimum_velocity; double decide_path_distance; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp index 29a4a8efa719f..0adda77a2ad72 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp @@ -46,7 +46,9 @@ using visualization_msgs::msg::MarkerArray; // TODO(sugahara) move to util PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2); -lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler, const bool left_side); +lanelet::ConstLanelets getPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance); PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp index 4ba2cb08b6341..55b8bdf777692 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp @@ -27,6 +27,7 @@ namespace behavior_path_planner { using autoware_auto_perception_msgs::msg::PredictedObjects; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; /* @@ -38,6 +39,8 @@ struct StartGoalPlannerData PredictedObjects filtered_objects; TargetObjectsOnLane target_objects_on_lane; std::vector ego_predicted_path; + // collision check debug map + CollisionCheckDebugMap collision_check; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp index 60bbb3c78b401..217b3748fa250 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" @@ -31,6 +32,7 @@ namespace behavior_path_planner::utils::start_goal_planner_common { using behavior_path_planner::StartPlannerParameters; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; @@ -63,7 +65,7 @@ void updateEgoPredictedPathParams( void updateEgoPredictedPathParams( std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & start_planner_params); + const std::shared_ptr & goal_planner_params); void updateSafetyCheckParams( std::shared_ptr & safety_check_params, @@ -71,7 +73,7 @@ void updateSafetyCheckParams( void updateSafetyCheckParams( std::shared_ptr & safety_check_params, - const std::shared_ptr & start_planner_params); + const std::shared_ptr & goal_planner_params); void updateObjectsFilteringParams( std::shared_ptr & objects_filtering_params, @@ -79,16 +81,38 @@ void updateObjectsFilteringParams( void updateObjectsFilteringParams( std::shared_ptr & objects_filtering_params, - const std::shared_ptr & start_planner_params); + const std::shared_ptr & goal_planner_params); void updatePathProperty( std::shared_ptr & ego_predicted_path_params, const std::pair & pairs_terminal_velocity_and_accel); +void initializeCollisionCheckDebugMap(CollisionCheckDebugMap & collision_check_debug_map); + +void updateSafetyCheckTargetObjectsData( + StartGoalPlannerData & data, const PredictedObjects & filtered_objects, + const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path); + std::pair getPairsTerminalVelocityAndAccel( const std::vector> & pairs_terminal_velocity_and_accel, const size_t current_path_idx); +/** + * @brief removeInverseOrderPathPoints function + * + * This function is designed to handle a situation that can arise when shifting paths on a curve, + * where the positions of the path points may become inverted (i.e., a point further along the path + * comes to be located before an earlier point). It corrects for this by using the function + * tier4_autoware_utils::isDrivingForward(p1, p2) to check if each pair of adjacent points is in + * the correct order (with the second point being 'forward' of the first). Any points which fail + * this test are omitted from the returned PathWithLaneId. + * + * @param path A path with points possibly in incorrect order. + * @return Returns a new PathWithLaneId that has all points in the correct order. + */ +PathWithLaneId removeInverseOrderPathPoints(const PathWithLaneId & path); + } // namespace behavior_path_planner::utils::start_goal_planner_common #endif // BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp index abdf17c9c6cfe..2b2de183b2dac 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp @@ -41,7 +41,7 @@ class FreespacePullOut : public PullOutPlannerBase PlannerType getPlannerType() override { return PlannerType::FREESPACE; } - boost::optional plan(Pose start_pose, Pose end_pose) override; + boost::optional plan(const Pose & start_pose, const Pose & end_pose) override; protected: std::unique_ptr planner_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp index 7ace770dc7ff1..a5ff52e74038f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp @@ -29,7 +29,7 @@ class GeometricPullOut : public PullOutPlannerBase explicit GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters); PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; }; - boost::optional plan(Pose start_pose, Pose goal_pose) override; + boost::optional plan(const Pose & start_pose, const Pose & goal_pose) override; GeometricParallelParking planner_; ParallelParkingParameters parallel_parking_parameters_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp index 6e6e5111e5dd9..cb662efd767bf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp @@ -60,7 +60,7 @@ class PullOutPlannerBase } virtual PlannerType getPlannerType() = 0; - virtual boost::optional plan(Pose start_pose, Pose goal_pose) = 0; + virtual boost::optional plan(const Pose & start_pose, const Pose & goal_pose) = 0; protected: std::shared_ptr planner_data_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp index 2042593064c51..0842d0a17bd97 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp @@ -37,7 +37,7 @@ class ShiftPullOut : public PullOutPlannerBase std::shared_ptr & lane_departure_checker); PlannerType getPlannerType() override { return PlannerType::SHIFT; }; - boost::optional plan(Pose start_pose, Pose goal_pose) override; + boost::optional plan(const Pose & start_pose, const Pose & goal_pose) override; std::vector calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp index 63e374e074a5a..d85e6181764d5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp @@ -16,6 +16,9 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_ #include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" +#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include @@ -36,6 +39,7 @@ namespace behavior_path_planner::start_planner_utils using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using route_handler::RouteHandler; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 0caa79ba5d062..d5e1d22fdcc88 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -22,11 +22,11 @@ #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index a9b137e54e90f..41df95378c52f 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -19,7 +19,7 @@ #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include +#include #include #include 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 bea1447b0d95a..b2c326fda7a69 100644 --- a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index d6a237079448f..b51fc585ce935 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -19,8 +19,9 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include #include - +#include namespace marker_utils { using behavior_path_planner::ShiftLine; diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 7708fd8ac116e..ee9c445e9fd2f 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include 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 78c063421f29a..eee87a09a94db 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 @@ -23,7 +23,10 @@ #include #include -#include +#include +#include +#include +#include #include #include @@ -392,7 +395,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de * STEP 2 * Modify the raw shift points. (Merging, Trimming) */ - const auto processed_raw_sp = applyPreProcessToRawShiftLines(data.unapproved_raw_sl, debug); + const auto processed_raw_sp = applyPreProcess(data.unapproved_raw_sl, debug); /** * STEP 3 @@ -530,9 +533,8 @@ void AvoidanceModule::fillEgoStatus( void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugData & debug) const { - debug.output_shift = data.candidate_path.shift_length; - debug.current_raw_shift = data.unapproved_raw_sl; - debug.new_shift_lines = data.unapproved_new_sl; + debug.step1_current_raw_shift_line = data.unapproved_raw_sl; + debug.step4_new_shift_line = data.unapproved_new_sl; if (!data.stop_target_object) { return; @@ -675,13 +677,13 @@ void AvoidanceModule::updateRegisteredRawShiftLines() printShiftLines(avoid_lines, "registered_raw_shift_lines_ (after)"); registered_raw_shift_lines_ = avoid_lines; - debug_data_.registered_raw_shift = registered_raw_shift_lines_; + debug_data_.step1_registered_shift_line = registered_raw_shift_lines_; } -AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines( +AvoidLineArray AvoidanceModule::applyPreProcess( AvoidLineArray & raw_shift_lines, DebugData & debug) const { - const auto fill_gap_shift_lines = getFillGapShiftLines(raw_shift_lines); + const auto fill_gap_shift_lines = applyFillGapProcess(raw_shift_lines, debug); /** * Use all registered points. For the current points, if the similar one of the current @@ -706,7 +708,7 @@ AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines( // It is temporally solved by changing the threshold of trimSimilarGrad, but it needs to be // fixed in a proper way. // Maybe after merge, all shift points before the prepare distance can be deleted. - addReturnShiftLineFromEgo(raw_shift_lines); + raw_shift_lines = addReturnShiftLine(raw_shift_lines, debug); /* * Add gap filled shift lines so that merged shift lines connect smoothly. @@ -714,7 +716,7 @@ AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines( fillShiftLineGap(raw_shift_lines); raw_shift_lines.insert( raw_shift_lines.end(), fill_gap_shift_lines.begin(), fill_gap_shift_lines.end()); - debug.gap_filled = raw_shift_lines; + debug.step1_filled_shift_line = raw_shift_lines; /** * On each path point, compute shift length with considering the raw shift points. @@ -725,8 +727,8 @@ AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines( * Note: Because this function just foolishly extracts points, it includes * insignificant small (useless) shift points, which should be removed in post-process. */ - auto merged_shift_lines = mergeShiftLines(raw_shift_lines, debug); - debug.merged = merged_shift_lines; + auto merged_shift_lines = applyMergeProcess(raw_shift_lines, debug); + debug.step2_merged_shift_line = merged_shift_lines; /* * Remove unnecessary shift points @@ -735,7 +737,7 @@ AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines( * - Combine shift points that have almost same gradient * - Remove unnecessary return shift (back to the center line). */ - auto shift_lines = trimShiftLine(merged_shift_lines, debug); + auto shift_lines = applyTrimProcess(merged_shift_lines, debug); DEBUG_PRINT("final shift point size = %lu", shift_lines.size()); return shift_lines; @@ -881,17 +883,35 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( } // output avoidance path under lateral jerk constraints. - const auto feasible_shift_length = PathShifter::calcLateralDistFromJerk( + const auto feasible_relative_shift_length = PathShifter::calcLateralDistFromJerk( remaining_distance, helper_.getLateralMaxJerkLimit(), helper_.getAvoidanceEgoSpeed()); - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 1000, - "original shift length is not feasible. generate avoidance path under the constraints. " - "[original: (%.2f) actual: (%.2f)]", - std::abs(avoiding_shift), feasible_shift_length); + if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) { + object.reason = "LessThanExecutionThreshold"; + return boost::none; + } + + const auto feasible_shift_length = + desire_shift_length > 0.0 ? feasible_relative_shift_length + current_ego_shift + : -1.0 * feasible_relative_shift_length + current_ego_shift; + + const auto feasible = + std::abs(feasible_shift_length - object.overhang_dist) < + 0.5 * planner_data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral; + if (feasible) { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 1000, "feasible shift length is not enough to avoid. "); + object.reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN; + return boost::none; + } - return desire_shift_length > 0.0 ? feasible_shift_length + current_ego_shift - : -1.0 * feasible_shift_length + current_ego_shift; + { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 1000, "use feasible shift length. [original: (%.2f) actual: (%.2f)]", + std::abs(avoiding_shift), feasible_relative_shift_length); + } + + return feasible_shift_length; }; const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; }; @@ -1201,7 +1221,8 @@ AvoidLineArray AvoidanceModule::extractShiftLinesFromLine(ShiftLineData & shift_ return merged_avoid_lines; } -AvoidLineArray AvoidanceModule::getFillGapShiftLines(const AvoidLineArray & shift_lines) const +AvoidLineArray AvoidanceModule::applyFillGapProcess( + const AvoidLineArray & shift_lines, [[maybe_unused]] DebugData & debug) const { AvoidLineArray ret{}; @@ -1280,7 +1301,7 @@ void AvoidanceModule::fillShiftLineGap(AvoidLineArray & shift_lines) const helper_.alignShiftLinesOrder(shift_lines, false); } -AvoidLineArray AvoidanceModule::mergeShiftLines( +AvoidLineArray AvoidanceModule::applyMergeProcess( const AvoidLineArray & raw_shift_lines, DebugData & debug) const { // Generate shift line by merging raw_shift_lines. @@ -1312,7 +1333,7 @@ AvoidLineArray AvoidanceModule::mergeShiftLines( return merged_shift_lines; } -AvoidLineArray AvoidanceModule::trimShiftLine( +AvoidLineArray AvoidanceModule::applyTrimProcess( const AvoidLineArray & shift_lines, DebugData & debug) const { if (shift_lines.empty()) { @@ -1327,41 +1348,37 @@ AvoidLineArray AvoidanceModule::trimShiftLine( // - Change the shift length to the previous one if the deviation is small. { constexpr double SHIFT_DIFF_THRES = 1.0; - trimSmallShiftLine(sl_array_trimmed, SHIFT_DIFF_THRES); + applySmallShiftFilter(sl_array_trimmed, SHIFT_DIFF_THRES); } // - Combine avoid points that have almost same gradient. // this is to remove the noise. { const auto THRESHOLD = parameters_->same_grad_filter_1_threshold; - trimSimilarGradShiftLine(sl_array_trimmed, THRESHOLD); - debug.trim_similar_grad_shift = sl_array_trimmed; - printShiftLines(sl_array_trimmed, "after trim_similar_grad_shift"); + applySimilarGradFilter(sl_array_trimmed, THRESHOLD); + debug.step3_grad_filtered_first = sl_array_trimmed; } // - Quantize the shift length to reduce the shift point noise // This is to remove the noise coming from detection accuracy, interpolation, resampling, etc. { const auto THRESHOLD = parameters_->quantize_filter_threshold; - quantizeShiftLine(sl_array_trimmed, THRESHOLD); - printShiftLines(sl_array_trimmed, "after sl_array_trimmed"); - debug.quantized = sl_array_trimmed; + applyQuantizeProcess(sl_array_trimmed, THRESHOLD); + debug.step3_quantized_shift_line = sl_array_trimmed; } // - Change the shift length to the previous one if the deviation is small. { constexpr double SHIFT_DIFF_THRES = 1.0; - trimSmallShiftLine(sl_array_trimmed, SHIFT_DIFF_THRES); - debug.trim_small_shift = sl_array_trimmed; - printShiftLines(sl_array_trimmed, "after trim_small_shift"); + applySmallShiftFilter(sl_array_trimmed, SHIFT_DIFF_THRES); + debug.step3_noise_removed = sl_array_trimmed; } // - Combine avoid points that have almost same gradient (again) { const auto THRESHOLD = parameters_->same_grad_filter_2_threshold; - trimSimilarGradShiftLine(sl_array_trimmed, THRESHOLD); - debug.trim_similar_grad_shift_second = sl_array_trimmed; - printShiftLines(sl_array_trimmed, "after trim_similar_grad_shift_second"); + applySimilarGradFilter(sl_array_trimmed, THRESHOLD); + debug.step3_grad_filtered_second = sl_array_trimmed; } // - trimTooSharpShift @@ -1369,23 +1386,21 @@ AvoidLineArray AvoidanceModule::trimShiftLine( // If the shift is sharp, it is combined with the next shift point until it gets non-sharp. { const auto THRESHOLD = parameters_->sharp_shift_filter_threshold; - trimSharpReturn(sl_array_trimmed, THRESHOLD); - debug.trim_too_sharp_shift = sl_array_trimmed; - printShiftLines(sl_array_trimmed, "after trimSharpReturn"); + applySharpShiftFilter(sl_array_trimmed, THRESHOLD); } // - Combine avoid points that have almost same gradient (again) { const auto THRESHOLD = parameters_->same_grad_filter_3_threshold; - trimSimilarGradShiftLine(sl_array_trimmed, THRESHOLD); - debug.trim_similar_grad_shift_third = sl_array_trimmed; - printShiftLines(sl_array_trimmed, "after trim_similar_grad_shift_second"); + applySimilarGradFilter(sl_array_trimmed, THRESHOLD); + debug.step3_grad_filtered_third = sl_array_trimmed; } return sl_array_trimmed; } -void AvoidanceModule::quantizeShiftLine(AvoidLineArray & shift_lines, const double threshold) const +void AvoidanceModule::applyQuantizeProcess( + AvoidLineArray & shift_lines, const double threshold) const { if (threshold < 1.0e-5) { return; // no need to process @@ -1398,7 +1413,8 @@ void AvoidanceModule::quantizeShiftLine(AvoidLineArray & shift_lines, const doub helper_.alignShiftLinesOrder(shift_lines); } -void AvoidanceModule::trimSmallShiftLine(AvoidLineArray & shift_lines, const double threshold) const +void AvoidanceModule::applySmallShiftFilter( + AvoidLineArray & shift_lines, const double threshold) const { if (shift_lines.empty()) { return; @@ -1416,7 +1432,7 @@ void AvoidanceModule::trimSmallShiftLine(AvoidLineArray & shift_lines, const dou } } -void AvoidanceModule::trimSimilarGradShiftLine( +void AvoidanceModule::applySimilarGradFilter( AvoidLineArray & avoid_lines, const double threshold) const { if (avoid_lines.empty()) { @@ -1480,7 +1496,8 @@ void AvoidanceModule::trimSimilarGradShiftLine( DEBUG_PRINT("size %lu -> %lu", input.size(), avoid_lines.size()); } -void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines, const double threshold) const +void AvoidanceModule::applySharpShiftFilter( + AvoidLineArray & shift_lines, const double threshold) const { AvoidLineArray shift_lines_orig = shift_lines; shift_lines.clear(); @@ -1617,30 +1634,31 @@ void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines, const double } helper_.alignShiftLinesOrder(shift_lines); - - DEBUG_PRINT("trimSharpReturn: size %lu -> %lu", shift_lines_orig.size(), shift_lines.size()); } -void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) const +AvoidLineArray AvoidanceModule::addReturnShiftLine( + const AvoidLineArray & shift_lines, DebugData & debug) const { constexpr double ep = 1.0e-3; const auto & data = avoid_data_; - const bool has_candidate_point = !sl_candidates.empty(); + const bool has_candidate_point = !shift_lines.empty(); const bool has_registered_point = !path_shifter_.getShiftLines().empty(); // If the return-to-center shift points are already registered, do nothing. if (!has_registered_point && std::fabs(getCurrentBaseShift()) < ep) { DEBUG_PRINT("No shift points, not base offset. Do not have to add return-shift."); - return; + return shift_lines; } constexpr double RETURN_SHIFT_THRESHOLD = 0.1; DEBUG_PRINT("registered last shift = %f", path_shifter_.getLastShiftLength()); if (std::abs(path_shifter_.getLastShiftLength()) < RETURN_SHIFT_THRESHOLD) { DEBUG_PRINT("Return shift is already registered. do nothing."); - return; + return shift_lines; } + AvoidLineArray ret = shift_lines; + // From here, the return-to-center is not registered. But perhaps the candidate is // already generated. @@ -1650,8 +1668,8 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) { // avoidance points: Yes, shift points: No -> select last avoidance point. if (has_candidate_point && !has_registered_point) { - helper_.alignShiftLinesOrder(sl_candidates, false); - last_sl = sl_candidates.back(); + helper_.alignShiftLinesOrder(ret, false); + last_sl = ret.back(); } // avoidance points: No, shift points: Yes -> select last shift point. @@ -1662,8 +1680,8 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) // avoidance points: Yes, shift points: Yes -> select the last one from both. if (has_candidate_point && has_registered_point) { - helper_.alignShiftLinesOrder(sl_candidates, false); - const auto & al = sl_candidates.back(); + helper_.alignShiftLinesOrder(ret, false); + const auto & al = ret.back(); const auto & sl = utils::avoidance::fillAdditionalInfo( data, AvoidLine{path_shifter_.getLastShiftLine().get()}); last_sl = (sl.end_longitudinal > al.end_longitudinal) ? sl : al; @@ -1677,7 +1695,6 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) last_sl.end_shift_length = getCurrentBaseShift(); } } - printShiftLines(ShiftLineArray{last_sl}, "last shift point"); // There already is a shift point candidates to go back to center line, but it could be too sharp // due to detection noise or timing. @@ -1686,7 +1703,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) const auto current_base_shift = helper_.getEgoShift(); if (std::abs(current_base_shift) < ep) { DEBUG_PRINT("last shift almost is zero, and current base_shift is zero. do nothing."); - return; + return ret; } // Is there a shift point in the opposite direction of the current_base_shift? @@ -1694,21 +1711,21 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) // the shift length are for return-shift. // Yes -> we can NOT overwrite, because it might be not a return-shift, but a avoiding // shift to the opposite direction which can not be overwritten by the return-shift. - for (const auto & sl : sl_candidates) { + for (const auto & sl : ret) { if ( (current_base_shift > 0.0 && sl.end_shift_length < -ep) || (current_base_shift < 0.0 && sl.end_shift_length > ep)) { DEBUG_PRINT( "try to put overwrite return shift, but there is shift for opposite direction. Skip " "adding return shift."); - return; + return ret; } } // If return shift already exists in candidate or registered shift lines, skip adding return // shift. if (has_candidate_point || has_registered_point) { - return; + return ret; } // set the return-shift from ego. @@ -1725,7 +1742,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) const auto nominal_avoid_distance = helper_.getMaxAvoidanceDistance(last_sl.end_shift_length); if (arclength_from_ego.empty()) { - return; + return ret; } const auto remaining_distance = arclength_from_ego.back() - parameters_->remain_buffer_distance; @@ -1736,7 +1753,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) // check if there is enough distance for return. if (last_sl_distance > remaining_distance) { // tmp: add some small number (+1.0) RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 1000, "No enough distance for return."); - return; + return ret; } // If the remaining distance is not enough, the return shift needs to be shrunk. @@ -1788,9 +1805,8 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) al.end_longitudinal = arclength_from_ego.at(al.end_idx); al.end_shift_length = last_sl.end_shift_length; al.start_shift_length = last_sl.end_shift_length; - sl_candidates.push_back(al); - printShiftLines(AvoidLineArray{al}, "prepare for return"); - debug_data_.extra_return_shift.push_back(al); + ret.push_back(al); + debug.step1_return_shift_line.push_back(al); } // shift point for return to center line @@ -1807,12 +1823,11 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) al.end_longitudinal = arclength_from_ego.at(al.end_idx); al.end_shift_length = 0.0; al.start_shift_length = last_sl.end_shift_length; - sl_candidates.push_back(al); - printShiftLines(AvoidLineArray{al}, "return point"); - debug_data_.extra_return_shift.push_back(al); + ret.push_back(al); + debug.step1_return_shift_line.push_back(al); } - DEBUG_PRINT("Return Shift is added."); + return ret; } bool AvoidanceModule::isSafePath( @@ -1883,10 +1898,9 @@ bool AvoidanceModule::isSafePath( safe_count_ = 0; return false; - } else { - marker_utils::updateCollisionCheckDebugMap(debug.collision_check, current_debug_data, true); } } + marker_utils::updateCollisionCheckDebugMap(debug.collision_check, current_debug_data, true); } safe_count_++; @@ -2453,6 +2467,9 @@ bool AvoidanceModule::isValidShiftLine( ShiftedPath proposed_shift_path; shifter_for_validate.generate(&proposed_shift_path); + debug_data_.proposed_spline_shift = proposed_shift_path.shift_length; + debug_data_.step4_valid_shift_line = shift_lines; + // check offset between new shift path and ego position. { const auto new_idx = planner_data_->findEgoIndex(proposed_shift_path.path.points); @@ -2467,8 +2484,6 @@ bool AvoidanceModule::isValidShiftLine( } } - debug_data_.proposed_spline_shift = proposed_shift_path.shift_length; - return true; // valid shift line. } @@ -2518,6 +2533,7 @@ void AvoidanceModule::updateData() void AvoidanceModule::processOnEntry() { initVariables(); + removeRTCStatus(); } void AvoidanceModule::processOnExit() @@ -2713,6 +2729,8 @@ void AvoidanceModule::updateDebugMarker( return; } + const auto & path = data.reference_path; + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); const auto add = [this](const MarkerArray & added) { appendMarkerArray(added, &debug_marker_); }; @@ -2731,20 +2749,16 @@ void AvoidanceModule::updateDebugMarker( add(createOtherObjectsMarkerArray(objects, ns)); }; - add(createEgoStatusMarkerArray(data, getEgoPose(), "ego_status")); - - const auto & path = data.reference_path; - add(createPathMarkerArray(debug.center_line, "centerline", 0, 0.0, 0.5, 0.9)); - add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); - add(createPathMarkerArray( - helper_.getPreviousLinearShiftPath().path, "prev_linear_shift", 0, 0.5, 0.4, 0.6)); - add(createPoseMarkerArray(data.reference_pose, "reference_pose", 0, 0.9, 0.3, 0.3)); - - 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)); + const auto addShiftLength = + [&](const auto & shift_length, const auto & ns, auto r, auto g, auto b) { + add(createShiftLengthMarkerArray(shift_length, path, ns, r, g, b)); + }; - add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang")); - add(createOverhangFurthestLineStringMarkerArray(debug.bounds, "bounds", 1.0, 0.0, 1.0)); + const auto addShiftGrad = [&]( + const auto & shift_grad, const auto & shift_length, const auto & ns, + auto r, auto g, auto b) { + add(createShiftGradMarkerArray(shift_grad, shift_length, path, ns, r, g, b)); + }; // ignore objects { @@ -2761,11 +2775,33 @@ void AvoidanceModule::updateDebugMarker( addObjects(data.other_objects, std::string("LessThanExecutionThreshold")); } - // parent object info + // shift line pre-process { - 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.step1_registered_shift_line, "step1_registered_shift_line", 1.0, 1.0, 1.0); + addAvoidLine(debug.step1_current_raw_shift_line, "step1_current_raw_shift_line", 0.9, 1.0, 1.0); + addAvoidLine(debug.step1_return_shift_line, "step1_return_shift_line", 0.8, 1.0, 1.0); + addAvoidLine(debug.step1_filled_shift_line, "step1_filled_shift_line", 0.7, 1.0, 1.0); + } + + // merge process + { + addAvoidLine(debug.step2_merged_shift_line, "step2_merged_shift_line", 0.0, 1.0, 1.0); + } + + // trimming process + { + addAvoidLine(debug.step3_grad_filtered_first, "step3_grad_filtered_first", 0.0, 0.0, 1.0); + addAvoidLine(debug.step3_grad_filtered_second, "step3_grad_filtered_second", 0.0, 0.1, 1.0); + addAvoidLine(debug.step3_grad_filtered_third, "step3_grad_filtered_third", 0.0, 0.2, 1.0); + addAvoidLine(debug.step3_quantized_shift_line, "step3_quantized_shift_line", 0.0, 0.3, 1.0); + addAvoidLine(debug.step3_noise_removed, "step3_noise_removed", 0.0, 0.4, 1.0); + } + + // registering process + { + addShiftLine(shifter.getShiftLines(), "step4_old_shift_line", 1.0, 1.0, 0.0, 0.3); + addAvoidLine(debug.step4_new_shift_line, "step4_new_shift_line", 1.0, 0.0, 0.0, 0.3); + addAvoidLine(debug.step4_valid_shift_line, "step4_valid_shift_line", 1.0, 0.0, 0.0, 0.3); } // safety check @@ -2777,58 +2813,28 @@ void AvoidanceModule::updateDebugMarker( // shift length { - const std::string ns = "shift_length"; - add(createShiftLengthMarkerArray(debug.pos_shift, path, ns + "_pos", 0.0, 0.7, 0.5)); - add(createShiftLengthMarkerArray(debug.neg_shift, path, ns + "_neg", 0.0, 0.5, 0.7)); - add(createShiftLengthMarkerArray(debug.total_shift, path, ns + "_total", 0.99, 0.4, 0.2)); + addShiftLength(debug.pos_shift, "merged_length_pos", 0.0, 0.7, 0.5); + addShiftLength(debug.neg_shift, "merged_length_neg", 0.0, 0.5, 0.7); + addShiftLength(debug.total_shift, "merged_length_total", 0.99, 0.4, 0.2); } // shift grad { - const std::string ns = "shift_grad"; - add(createShiftGradMarkerArray( - debug.pos_shift_grad, debug.pos_shift, path, ns + "_pos", 0.0, 0.7, 0.5)); - add(createShiftGradMarkerArray( - debug.neg_shift_grad, debug.neg_shift, path, ns + "_neg", 0.0, 0.5, 0.7)); - add(createShiftGradMarkerArray( - debug.total_forward_grad, debug.total_shift, path, ns + "_total_forward", 0.99, 0.4, 0.2)); - add(createShiftGradMarkerArray( - debug.total_backward_grad, debug.total_shift, path, ns + "_total_backward", 0.4, 0.2, 0.99)); - } - - // shift path - { - const std::string ns = "shift_line"; - add(createShiftLengthMarkerArray( - helper_.getPreviousLinearShiftPath().shift_length, path, ns + "_linear_registered", 0.9, 0.3, - 0.3)); - add(createShiftLengthMarkerArray( - debug.proposed_spline_shift, path, ns + "_spline_proposed", 1.0, 1.0, 1.0)); + addShiftGrad(debug.pos_shift_grad, debug.pos_shift, "merged_grad_pos", 0.0, 0.7, 0.5); + addShiftGrad(debug.neg_shift_grad, debug.neg_shift, "merged_grad_neg", 0.0, 0.5, 0.7); + addShiftGrad(debug.total_forward_grad, debug.total_shift, "grad_forward", 0.99, 0.4, 0.2); + addShiftGrad(debug.total_backward_grad, debug.total_shift, "grad_backward", 0.4, 0.2, 0.9); } - // child shift points + // misc { - const std::string ns = "pipeline"; - add(createAvoidLineMarkerArray(debug.gap_filled, ns + "_1_gap_filled", 0.5, 0.8, 1.0, 0.05)); - add(createAvoidLineMarkerArray(debug.merged, ns + "_2_merge", 0.345, 0.968, 1.0, 0.05)); - add(createAvoidLineMarkerArray( - debug.trim_similar_grad_shift, ns + "_3_concat_by_grad", 0.976, 0.328, 0.910, 0.05)); - add( - createAvoidLineMarkerArray(debug.quantized, ns + "_4_quantized", 0.505, 0.745, 0.969, 0.05)); - add(createAvoidLineMarkerArray( - debug.trim_small_shift, ns + "_5_trim_small_shift", 0.663, 0.525, 0.941, 0.05)); - add(createAvoidLineMarkerArray( - debug.trim_similar_grad_shift_second, ns + "_6_concat_by_grad", 0.97, 0.32, 0.91, 0.05)); - add(createAvoidLineMarkerArray( - debug.trim_momentary_return, ns + "_7_trim_momentary_return", 0.976, 0.078, 0.878, 0.05)); - add(createAvoidLineMarkerArray( - debug.trim_too_sharp_shift, ns + "_8_trim_sharp_shift", 0.576, 0.0, 0.978, 0.05)); - add(createAvoidLineMarkerArray( - debug.trim_similar_grad_shift_third, ns + "_9_concat_by_grad", 1.0, 0.0, 0.0, 0.05)); - } - - addShiftLine(shifter.getShiftLines(), "path_shifter_registered_points", 0.99, 0.99, 0.0, 0.5); - addAvoidLine(debug.new_shift_lines, "path_shifter_proposed_points", 0.99, 0.0, 0.0, 0.5); + add(createEgoStatusMarkerArray(data, getEgoPose(), "ego_status")); + add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); + 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(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang")); + add(createOverhangFurthestLineStringMarkerArray(debug.bounds, "bounds", 1.0, 0.0, 1.0)); + } } void AvoidanceModule::updateAvoidanceDebugData( diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 8900e2b3e827c..912f867fd5701 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -14,6 +14,9 @@ #include "behavior_path_planner/scene_module/avoidance/manager.hpp" +#include "tier4_autoware_utils/ros/parameter.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + #include #include diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index da98c0478e580..b5ecffbcc9791 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index 82ebf59ea5bb2..616194ea66ac7 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -14,6 +14,8 @@ #include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + #include #include 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 7b86cb5c16440..bed4aecf733b9 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 @@ -16,17 +16,17 @@ #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" #include #include #include -#include #include -#include #include #include @@ -252,12 +252,29 @@ void GoalPlannerModule::initializeOccupancyGridMap() occupancy_grid_map_->setParam(occupancy_grid_map_param); } +void GoalPlannerModule::initializeSafetyCheckParameters() +{ + utils::start_goal_planner_common::updateEgoPredictedPathParams( + ego_predicted_path_params_, parameters_); + utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); + utils::start_goal_planner_common::updateObjectsFilteringParams( + objects_filtering_params_, parameters_); +} + void GoalPlannerModule::processOnEntry() { // Initialize occupancy grid map - if (parameters_->use_occupancy_grid) { + if ( + parameters_->use_occupancy_grid_for_goal_search || + parameters_->use_occupancy_grid_for_path_collision_check) { initializeOccupancyGridMap(); } + // Initialize safety checker + if (parameters_->safety_check_params.enable_safety_check) { + initializeSafetyCheckParameters(); + utils::start_goal_planner_common::initializeCollisionCheckDebugMap( + goal_planner_data_.collision_check); + } } void GoalPlannerModule::processOnExit() @@ -275,7 +292,7 @@ bool GoalPlannerModule::isExecutionRequested() const const auto & route_handler = planner_data_->route_handler; const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const Pose & goal_pose = route_handler->getOriginalGoalPose(); + const Pose goal_pose = route_handler->getOriginalGoalPose(); // check if goal_pose is in current_lanes. lanelet::ConstLanelet current_lane{}; @@ -306,12 +323,19 @@ bool GoalPlannerModule::isExecutionRequested() const return false; } + // if goal modification is not allowed + // 1) goal_pose is in current_lanes, plan path to the original fixed goal + // 2) goal_pose is NOT in current_lanes, do not execute goal_planner + if (!goal_planner_utils::isAllowedGoalModification(route_handler)) { + return goal_is_in_current_lanes; + } + // if goal arc coordinates can be calculated, check if goal is in request_length const double self_to_goal_arc_length = utils::getSignedDistance(current_pose, goal_pose, current_lanes); const double request_length = goal_planner_utils::isAllowedGoalModification(route_handler) ? calcModuleRequestLength() - : parameters_->minimum_request_length; + : parameters_->pull_over_minimum_request_length; if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) { // if current position is far from goal or behind goal, do not execute goal_planner return false; @@ -327,8 +351,9 @@ bool GoalPlannerModule::isExecutionRequested() const // if (A) or (B) is met execute pull over // (A) target lane is `road` and same to the current lanes // (B) target lane is `road_shoulder` and neighboring to the current lanes - const lanelet::ConstLanelets pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_); + const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); lanelet::ConstLanelet target_lane{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); if (!isCrossingPossible(current_lane, target_lane)) { @@ -340,6 +365,17 @@ bool GoalPlannerModule::isExecutionRequested() const bool GoalPlannerModule::isExecutionReady() const { + // TODO(Sugahara): should check safe or not but in the current flow, it is not possible. + if (status_.pull_over_path == nullptr) { + return true; + } + + if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) { + if (!isSafePath()) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); + return false; + } + } return true; } @@ -348,13 +384,13 @@ double GoalPlannerModule::calcModuleRequestLength() const const auto min_stop_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { - return parameters_->minimum_request_length; + return parameters_->pull_over_minimum_request_length; } const double minimum_request_length = *min_stop_distance + parameters_->backward_goal_search_length + approximate_pull_over_distance_; - return std::max(minimum_request_length, parameters_->minimum_request_length); + return std::max(minimum_request_length, parameters_->pull_over_minimum_request_length); } Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const @@ -363,8 +399,9 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const const double base_link2front = planner_data_->parameters.base_link2front; const double base_link2rear = planner_data_->parameters.base_link2rear; - const lanelet::ConstLanelets pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); lanelet::Lanelet closest_pull_over_lanelet{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &closest_pull_over_lanelet); @@ -619,8 +656,9 @@ void GoalPlannerModule::setLanes() planner_data_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length, /*forward_only_in_route*/ false); - status_.pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + status_.pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); status_.lanes = utils::generateDrivableLanesWithShoulderLanes(status_.current_lanes, status_.pull_over_lanes); } @@ -628,18 +666,25 @@ void GoalPlannerModule::setLanes() void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) { if (status_.is_safe_static_objects) { - // clear stop pose when the path is safe and activated - if (isActivated()) { - resetWallPoses(); + if (isSafePath()) { + // clear stop pose when the path is safe against static/dynamic objects and activated + if (isActivated()) { + resetWallPoses(); + } + // keep stop if not enough time passed, + // because it takes time for the trajectory to be reflected + auto current_path = getCurrentPath(); + keepStoppedWithCurrentPath(current_path); + + output.path = std::make_shared(current_path); + output.reference_path = getPreviousModuleOutput().reference_path; + } else if (status_.has_decided_path && isActivated()) { + // situation : not safe against dynamic objects after approval + // insert stop point in current path if ego is able to stop with acceleration and jerk + // constraints + setStopPathFromCurrentPath(output); } - // keep stop if not enough time passed, - // because it takes time for the trajectory to be reflected - auto current_path = getCurrentPath(); - keepStoppedWithCurrentPath(current_path); - - output.path = std::make_shared(current_path); - output.reference_path = getPreviousModuleOutput().reference_path; } else { // not safe: use stop_path setStopPath(output); @@ -657,6 +702,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. status_.prev_is_safe = status_.is_safe_static_objects; + status_.prev_is_safe_dynamic_objects = status_.is_safe_dynamic_objects; } void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) @@ -664,7 +710,6 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) if (status_.prev_is_safe || status_.prev_stop_path == nullptr) { // safe -> not_safe or no prev_stop_path: generate new stop_path output.path = std::make_shared(generateStopPath()); - output.reference_path = getPreviousModuleOutput().reference_path; status_.prev_stop_path = output.path; // set stop path as pull over path mutex_.lock(); @@ -679,10 +724,34 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) } else { // not_safe -> not_safe: use previous stop path output.path = status_.prev_stop_path; - output.reference_path = getPreviousModuleOutput().reference_path; RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); } + output.reference_path = getPreviousModuleOutput().reference_path; +} + +void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output) +{ + // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path + if (status_.prev_is_safe_dynamic_objects || status_.prev_stop_path_after_approval == nullptr) { + if (const auto stop_inserted_path = generateStopInsertedCurrentPath()) { + output.path = std::make_shared(*stop_inserted_path); + status_.prev_stop_path_after_approval = output.path; + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); + } else { + output.path = std::make_shared(getCurrentPath()); + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, + "Collision detected, no feasible stop path found, cannot stop."); + } + std::lock_guard lock(mutex_); + last_path_update_time_ = std::make_unique(clock_->now()); + } else { + // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path + output.path = status_.prev_stop_path_after_approval; + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path"); + } + output.reference_path = getPreviousModuleOutput().reference_path; } void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const @@ -1044,6 +1113,34 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() return stop_path; } +std::optional GoalPlannerModule::generateStopInsertedCurrentPath() +{ + auto current_path = getCurrentPath(); + if (current_path.points.empty()) { + return {}; + } + + // try to insert stop point in current_path after approval + // but if can't stop with constraints(maximum deceleration, maximum jerk), don't insert stop point + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); + if (!min_stop_distance) { + return {}; + } + + // set stop point + const auto stop_idx = motion_utils::insertStopPoint( + planner_data_->self_odometry->pose.pose, *min_stop_distance, current_path.points); + + if (!stop_idx) { + return {}; + } else { + stop_pose_ = current_path.points.at(*stop_idx).point.pose; + } + + return current_path; +} + void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath() { if (isActivated() && last_approved_time_ != nullptr) { @@ -1071,6 +1168,10 @@ bool GoalPlannerModule::incrementPathIndex() PathWithLaneId GoalPlannerModule::getCurrentPath() const { + if (status_.pull_over_path == nullptr) { + return PathWithLaneId{}; + } + if (status_.pull_over_path->partial_paths.size() <= status_.current_path_idx) { return PathWithLaneId{}; } @@ -1128,7 +1229,7 @@ bool GoalPlannerModule::isStuck() bool GoalPlannerModule::hasFinishedCurrentPath() { - const auto & current_path_end = getCurrentPath().points.back(); + const auto current_path_end = getCurrentPath().points.back(); const auto & self_pose = planner_data_->self_odometry->pose.pose; const bool is_near_target = tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < parameters_->th_arrived_distance; @@ -1193,7 +1294,7 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const { - if (parameters_->use_occupancy_grid || !occupancy_grid_map_) { + if (parameters_->use_occupancy_grid_for_path_collision_check && occupancy_grid_map_) { const bool check_out_of_range = false; if (occupancy_grid_map_->hasObstacleOnPath(path, check_out_of_range)) { return true; @@ -1201,12 +1302,20 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const } if (parameters_->use_object_recognition) { + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); + const auto [pull_over_lane_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_over_lanes); + const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_over_lane_objects, parameters_->th_moving_object_velocity); const auto common_parameters = planner_data_->parameters; const double base_link2front = common_parameters.base_link2front; const double base_link2rear = common_parameters.base_link2rear; const double vehicle_width = common_parameters.vehicle_width; if (utils::path_safety_checker::checkCollisionWithExtraStoppingMargin( - path, *(planner_data_->dynamic_object), base_link2front, base_link2rear, vehicle_width, + path, pull_over_lane_stop_objects, base_link2front, base_link2rear, vehicle_width, parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_margin, parameters_->object_recognition_collision_check_max_extra_stopping_margin)) { return true; @@ -1441,12 +1550,100 @@ bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) return isCrossingPossible(start_pose, end_pose, lanes); } +void GoalPlannerModule::updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const +{ + goal_planner_data_.filtered_objects = filtered_objects; + goal_planner_data_.target_objects_on_lane = target_objects_on_lane; + goal_planner_data_.ego_predicted_path = ego_predicted_path; +} + +bool GoalPlannerModule::isSafePath() const +{ + const auto pull_over_path = getCurrentPath(); + const auto & current_pose = planner_data_->self_odometry->pose.pose; + const double current_velocity = std::hypot( + planner_data_->self_odometry->twist.twist.linear.x, + planner_data_->self_odometry->twist.twist.linear.y); + const auto & dynamic_object = planner_data_->dynamic_object; + const auto & route_handler = planner_data_->route_handler; + const double backward_path_length = planner_data_->parameters.backward_path_length; + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_over_path.points); + const std::pair terminal_velocity_and_accel = + utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( + status_.pull_over_path->pairs_terminal_velocity_and_accel, status_.current_path_idx); + RCLCPP_DEBUG( + getLogger(), "pairs_terminal_velocity_and_accel for goal_planner: %f, %f", + terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); + RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.current_path_idx); + utils::start_goal_planner_common::updatePathProperty( + ego_predicted_path_params_, terminal_velocity_and_accel); + const auto ego_predicted_path = + behavior_path_planner::utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params_, pull_over_path.points, current_pose, current_velocity, + ego_seg_idx); + + // filtering objects with velocity, position and class + const auto & filtered_objects = utils::path_safety_checker::filterObjects( + dynamic_object, route_handler, pull_over_lanes, current_pose.position, + objects_filtering_params_); + + // filtering objects based on the current position's lane + const auto & target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_); + + const double hysteresis_factor = + status_.is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate; + + utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( + goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); + + bool is_safe_dynamic_objects = true; + // Check for collisions with each predicted path of the object + for (const auto & object : target_objects_on_lane.on_current_lane) { + auto current_debug_data = marker_utils::createObjectDebug(object); + + bool is_safe_dynamic_object = true; + + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + object, objects_filtering_params_->check_all_predicted_path); + + // If a collision is detected, mark the object as unsafe and break the loop + for (const auto & obj_path : obj_predicted_paths) { + if (!utils::path_safety_checker::checkCollision( + pull_over_path, ego_predicted_path, object, obj_path, planner_data_->parameters, + safety_check_params_->rss_params, hysteresis_factor, current_debug_data.second)) { + marker_utils::updateCollisionCheckDebugMap( + goal_planner_data_.collision_check, current_debug_data, false); + is_safe_dynamic_objects = false; + is_safe_dynamic_object = false; + break; + } + } + if (is_safe_dynamic_object) { + marker_utils::updateCollisionCheckDebugMap( + goal_planner_data_.collision_check, current_debug_data, is_safe_dynamic_object); + } + } + + return is_safe_dynamic_objects; +} + void GoalPlannerModule::setDebugData() { debug_marker_.markers.clear(); + using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; + using marker_utils::createPredictedPathMarkerArray; using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; @@ -1488,6 +1685,17 @@ void GoalPlannerModule::setDebugData() add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } + if (goal_planner_data_.ego_predicted_path.size() > 0) { + const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( + goal_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); + add(createPredictedPathMarkerArray( + ego_predicted_path, vehicle_info_, "ego_predicted_path", 0, 0.0, 0.5, 0.9)); + } + + if (goal_planner_data_.filtered_objects.objects.size() > 0) { + add(createObjectsMarkerArray( + goal_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + } } // Visualize planner type text @@ -1550,8 +1758,9 @@ bool GoalPlannerModule::checkOriginalGoalIsInShoulder() const const auto & route_handler = planner_data_->route_handler; const Pose & goal_pose = route_handler->getOriginalGoalPose(); - const lanelet::ConstLanelets pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_); + const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); lanelet::ConstLanelet target_lane{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 209b12c42537c..9d794b178b60e 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -15,8 +15,8 @@ #include "behavior_path_planner/scene_module/goal_planner/manager.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" -#include #include #include @@ -32,18 +32,17 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( { GoalPlannerParameters p; + const std::string base_ns = "goal_planner."; // general params { - std::string ns = "goal_planner."; - p.minimum_request_length = node->declare_parameter(ns + "minimum_request_length"); - p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); - p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); - p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); + p.th_stopped_velocity = node->declare_parameter(base_ns + "th_stopped_velocity"); + p.th_arrived_distance = node->declare_parameter(base_ns + "th_arrived_distance"); + p.th_stopped_time = node->declare_parameter(base_ns + "th_stopped_time"); } // goal search { - std::string ns = "goal_planner.goal_search."; + const std::string ns = base_ns + "goal_search."; p.search_priority = node->declare_parameter(ns + "search_priority"); p.forward_goal_search_length = node->declare_parameter(ns + "forward_goal_search_length"); @@ -72,10 +71,13 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // occupancy grid map { - std::string ns = "goal_planner.occupancy_grid."; - p.use_occupancy_grid = node->declare_parameter(ns + "use_occupancy_grid"); - p.use_occupancy_grid_for_longitudinal_margin = - node->declare_parameter(ns + "use_occupancy_grid_for_longitudinal_margin"); + const std::string ns = base_ns + "occupancy_grid."; + p.use_occupancy_grid_for_goal_search = + node->declare_parameter(ns + "use_occupancy_grid_for_goal_search"); + p.use_occupancy_grid_for_path_collision_check = + node->declare_parameter(ns + "use_occupancy_grid_for_path_collision_check"); + p.use_occupancy_grid_for_goal_longitudinal_margin = + node->declare_parameter(ns + "use_occupancy_grid_for_goal_longitudinal_margin"); p.occupancy_grid_collision_check_margin = node->declare_parameter(ns + "occupancy_grid_collision_check_margin"); p.theta_size = node->declare_parameter(ns + "theta_size"); @@ -84,18 +86,21 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // object recognition { - std::string ns = "goal_planner.object_recognition."; + const std::string ns = base_ns + "object_recognition."; p.use_object_recognition = node->declare_parameter(ns + "use_object_recognition"); p.object_recognition_collision_check_margin = node->declare_parameter(ns + "object_recognition_collision_check_margin"); p.object_recognition_collision_check_max_extra_stopping_margin = node->declare_parameter( ns + "object_recognition_collision_check_max_extra_stopping_margin"); + p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); } // pull over general params { - std::string ns = "goal_planner.pull_over."; + const std::string ns = base_ns + "pull_over."; + p.pull_over_minimum_request_length = + node->declare_parameter(ns + "minimum_request_length"); p.pull_over_velocity = node->declare_parameter(ns + "pull_over_velocity"); p.pull_over_minimum_velocity = node->declare_parameter(ns + "pull_over_minimum_velocity"); @@ -106,7 +111,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // shift parking { - std::string ns = "goal_planner.pull_over.shift_parking."; + const std::string ns = base_ns + "pull_over.shift_parking."; p.enable_shift_parking = node->declare_parameter(ns + "enable_shift_parking"); p.shift_sampling_num = node->declare_parameter(ns + "shift_sampling_num"); p.maximum_lateral_jerk = node->declare_parameter(ns + "maximum_lateral_jerk"); @@ -118,7 +123,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // forward parallel parking forward { - std::string ns = "goal_planner.pull_over.parallel_parking.forward."; + const std::string ns = base_ns + "pull_over.parallel_parking.forward."; p.enable_arc_forward_parking = node->declare_parameter(ns + "enable_arc_forward_parking"); p.parallel_parking_parameters.after_forward_parking_straight_distance = node->declare_parameter(ns + "after_forward_parking_straight_distance"); @@ -134,7 +139,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // forward parallel parking backward { - std::string ns = "goal_planner.pull_over.parallel_parking.backward."; + const std::string ns = base_ns + "pull_over.parallel_parking.backward."; p.enable_arc_backward_parking = node->declare_parameter(ns + "enable_arc_backward_parking"); p.parallel_parking_parameters.after_backward_parking_straight_distance = @@ -151,7 +156,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking general params { - std::string ns = "goal_planner.pull_over.freespace_parking."; + const std::string ns = base_ns + "pull_over.freespace_parking."; p.enable_freespace_parking = node->declare_parameter(ns + "enable_freespace_parking"); p.freespace_parking_algorithm = node->declare_parameter(ns + "freespace_parking_algorithm"); @@ -174,7 +179,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking search config { - std::string ns = "goal_planner.pull_over.freespace_parking.search_configs."; + const std::string ns = base_ns + "pull_over.freespace_parking.search_configs."; p.freespace_parking_common_parameters.theta_size = node->declare_parameter(ns + "theta_size"); p.freespace_parking_common_parameters.angle_goal_range = @@ -191,14 +196,14 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking costmap configs { - std::string ns = "goal_planner.pull_over.freespace_parking.costmap_configs."; + const std::string ns = base_ns + "pull_over.freespace_parking.costmap_configs."; p.freespace_parking_common_parameters.obstacle_threshold = node->declare_parameter(ns + "obstacle_threshold"); } // freespace parking astar { - std::string ns = "goal_planner.pull_over.freespace_parking.astar."; + const std::string ns = base_ns + "pull_over.freespace_parking.astar."; p.astar_parameters.only_behind_solutions = node->declare_parameter(ns + "only_behind_solutions"); p.astar_parameters.use_back = node->declare_parameter(ns + "use_back"); @@ -208,7 +213,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking rrtstar { - std::string ns = "goal_planner.pull_over.freespace_parking.rrtstar."; + const std::string ns = base_ns + "pull_over.freespace_parking.rrtstar."; p.rrt_star_parameters.enable_update = node->declare_parameter(ns + "enable_update"); p.rrt_star_parameters.use_informed_sampling = node->declare_parameter(ns + "use_informed_sampling"); @@ -218,9 +223,125 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); } + // stop condition + { + p.maximum_deceleration_for_stop = + node->declare_parameter(base_ns + "stop_condition.maximum_deceleration_for_stop"); + p.maximum_jerk_for_stop = + node->declare_parameter(base_ns + "stop_condition.maximum_jerk_for_stop"); + } + + std::string path_safety_check_ns = "goal_planner.path_safety_check."; + + // EgoPredictedPath + std::string ego_path_ns = path_safety_check_ns + "ego_predicted_path."; + { + p.ego_predicted_path_params.acceleration = + node->declare_parameter(ego_path_ns + "acceleration"); + p.ego_predicted_path_params.time_horizon = + node->declare_parameter(ego_path_ns + "time_horizon"); + p.ego_predicted_path_params.time_resolution = + node->declare_parameter(ego_path_ns + "time_resolution"); + p.ego_predicted_path_params.min_slow_speed = + node->declare_parameter(ego_path_ns + "min_slow_speed"); + p.ego_predicted_path_params.delay_until_departure = + node->declare_parameter(ego_path_ns + "delay_until_departure"); + p.ego_predicted_path_params.target_velocity = + node->declare_parameter(ego_path_ns + "target_velocity"); + } + + // ObjectFilteringParams + std::string obj_filter_ns = path_safety_check_ns + "target_filtering."; + { + p.objects_filtering_params.safety_check_time_horizon = + node->declare_parameter(obj_filter_ns + "safety_check_time_horizon"); + p.objects_filtering_params.safety_check_time_resolution = + node->declare_parameter(obj_filter_ns + "safety_check_time_resolution"); + p.objects_filtering_params.object_check_forward_distance = + node->declare_parameter(obj_filter_ns + "object_check_forward_distance"); + p.objects_filtering_params.object_check_backward_distance = + node->declare_parameter(obj_filter_ns + "object_check_backward_distance"); + p.objects_filtering_params.ignore_object_velocity_threshold = + node->declare_parameter(obj_filter_ns + "ignore_object_velocity_threshold"); + p.objects_filtering_params.include_opposite_lane = + node->declare_parameter(obj_filter_ns + "include_opposite_lane"); + p.objects_filtering_params.invert_opposite_lane = + node->declare_parameter(obj_filter_ns + "invert_opposite_lane"); + p.objects_filtering_params.check_all_predicted_path = + node->declare_parameter(obj_filter_ns + "check_all_predicted_path"); + p.objects_filtering_params.use_all_predicted_path = + node->declare_parameter(obj_filter_ns + "use_all_predicted_path"); + p.objects_filtering_params.use_predicted_path_outside_lanelet = + node->declare_parameter(obj_filter_ns + "use_predicted_path_outside_lanelet"); + } + + // ObjectTypesToCheck + std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + { + p.objects_filtering_params.object_types_to_check.check_car = + node->declare_parameter(obj_types_ns + "check_car"); + p.objects_filtering_params.object_types_to_check.check_truck = + node->declare_parameter(obj_types_ns + "check_truck"); + p.objects_filtering_params.object_types_to_check.check_bus = + node->declare_parameter(obj_types_ns + "check_bus"); + p.objects_filtering_params.object_types_to_check.check_trailer = + node->declare_parameter(obj_types_ns + "check_trailer"); + p.objects_filtering_params.object_types_to_check.check_unknown = + node->declare_parameter(obj_types_ns + "check_unknown"); + p.objects_filtering_params.object_types_to_check.check_bicycle = + node->declare_parameter(obj_types_ns + "check_bicycle"); + p.objects_filtering_params.object_types_to_check.check_motorcycle = + node->declare_parameter(obj_types_ns + "check_motorcycle"); + p.objects_filtering_params.object_types_to_check.check_pedestrian = + node->declare_parameter(obj_types_ns + "check_pedestrian"); + } + + // ObjectLaneConfiguration + std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + { + p.objects_filtering_params.object_lane_configuration.check_current_lane = + node->declare_parameter(obj_lane_ns + "check_current_lane"); + p.objects_filtering_params.object_lane_configuration.check_right_lane = + node->declare_parameter(obj_lane_ns + "check_right_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_left_lane = + node->declare_parameter(obj_lane_ns + "check_left_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = + node->declare_parameter(obj_lane_ns + "check_shoulder_lane"); + p.objects_filtering_params.object_lane_configuration.check_other_lane = + node->declare_parameter(obj_lane_ns + "check_other_lane"); + } + + // SafetyCheckParams + std::string safety_check_ns = path_safety_check_ns + "safety_check_params."; + { + p.safety_check_params.enable_safety_check = + node->declare_parameter(safety_check_ns + "enable_safety_check"); + p.safety_check_params.backward_path_length = + node->declare_parameter(safety_check_ns + "backward_path_length"); + p.safety_check_params.forward_path_length = + node->declare_parameter(safety_check_ns + "forward_path_length"); + p.safety_check_params.publish_debug_marker = + node->declare_parameter(safety_check_ns + "publish_debug_marker"); + } + + // RSSparams + std::string rss_ns = safety_check_ns + "rss_params."; + { + p.safety_check_params.rss_params.rear_vehicle_reaction_time = + node->declare_parameter(rss_ns + "rear_vehicle_reaction_time"); + p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = + node->declare_parameter(rss_ns + "rear_vehicle_safety_time_margin"); + p.safety_check_params.rss_params.lateral_distance_max_threshold = + node->declare_parameter(rss_ns + "lateral_distance_max_threshold"); + p.safety_check_params.rss_params.longitudinal_distance_min_threshold = + node->declare_parameter(rss_ns + "longitudinal_distance_min_threshold"); + p.safety_check_params.rss_params.longitudinal_velocity_delta_time = + node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); + } + // debug { - std::string ns = "goal_planner.debug."; + const std::string ns = base_ns + "debug."; p.print_debug_info = node->declare_parameter(ns + "print_debug_info"); } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 27f44433c6d4e..f4a75daa6c764 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -14,6 +14,9 @@ #include "behavior_path_planner/scene_module/lane_change/manager.hpp" +#include "tier4_autoware_utils/ros/parameter.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + #include #include diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 8deee76656dca..a91da98b11999 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -665,6 +666,8 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); const auto target_backward_polygon = utils::lane_change::createPolygon( target_backward_lanes, 0.0, std::numeric_limits::max()); + const auto dist_ego_to_current_lanes_center = + lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); LaneChangeTargetObjectIndices filtered_obj_indices; for (size_t i = 0; i < objects.objects.size(); ++i) { @@ -693,6 +696,14 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( min_dist_ego_to_obj = std::min(dist_ego_to_obj, min_dist_ego_to_obj); } + const auto is_lateral_far = [&]() { + const auto dist_object_to_current_lanes_center = + lanelet::utils::getLateralDistanceToClosestLanelet( + current_lanes, object.kinematics.initial_pose_with_covariance.pose); + const auto lateral = dist_object_to_current_lanes_center - dist_ego_to_current_lanes_center; + return std::abs(lateral) > (common_parameters.vehicle_width / 2); + }; + // ignore static object that are behind the ego vehicle if (obj_velocity_norm < 1.0 && max_dist_ego_to_obj < 0.0) { continue; @@ -703,8 +714,10 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( // TODO(watanabe): ignore static parked object that are in front of the ego vehicle in target // lanes - filtered_obj_indices.target_lane.push_back(i); - continue; + if (max_dist_ego_to_obj >= 0 || is_lateral_far()) { + filtered_obj_indices.target_lane.push_back(i); + continue; + } } // check if the object intersects with target backward lanes diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp index 5a039a98f2e52..288d9d44aee54 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp @@ -14,6 +14,8 @@ #include "behavior_path_planner/scene_module/side_shift/manager.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + #include #include diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index 9d2da0d13f56b..09166cd72a83e 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -14,6 +14,8 @@ #include "behavior_path_planner/scene_module/start_planner/manager.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + #include #include @@ -148,6 +150,122 @@ StartPlannerModuleManager::StartPlannerModuleManager( p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); } + // stop condition + { + p.maximum_deceleration_for_stop = + node->declare_parameter(ns + "stop_condition.maximum_deceleration_for_stop"); + p.maximum_jerk_for_stop = + node->declare_parameter(ns + "stop_condition.maximum_jerk_for_stop"); + } + + std::string base_ns = "start_planner.path_safety_check."; + + // EgoPredictedPath + std::string ego_path_ns = base_ns + "ego_predicted_path."; + { + p.ego_predicted_path_params.acceleration = + node->declare_parameter(ego_path_ns + "acceleration"); + p.ego_predicted_path_params.time_horizon = + node->declare_parameter(ego_path_ns + "time_horizon"); + p.ego_predicted_path_params.time_resolution = + node->declare_parameter(ego_path_ns + "time_resolution"); + p.ego_predicted_path_params.min_slow_speed = + node->declare_parameter(ego_path_ns + "min_slow_speed"); + p.ego_predicted_path_params.delay_until_departure = + node->declare_parameter(ego_path_ns + "delay_until_departure"); + p.ego_predicted_path_params.target_velocity = + node->declare_parameter(ego_path_ns + "target_velocity"); + } + + // ObjectFilteringParams + std::string obj_filter_ns = base_ns + "target_filtering."; + { + p.objects_filtering_params.safety_check_time_horizon = + node->declare_parameter(obj_filter_ns + "safety_check_time_horizon"); + p.objects_filtering_params.safety_check_time_resolution = + node->declare_parameter(obj_filter_ns + "safety_check_time_resolution"); + p.objects_filtering_params.object_check_forward_distance = + node->declare_parameter(obj_filter_ns + "object_check_forward_distance"); + p.objects_filtering_params.object_check_backward_distance = + node->declare_parameter(obj_filter_ns + "object_check_backward_distance"); + p.objects_filtering_params.ignore_object_velocity_threshold = + node->declare_parameter(obj_filter_ns + "ignore_object_velocity_threshold"); + p.objects_filtering_params.include_opposite_lane = + node->declare_parameter(obj_filter_ns + "include_opposite_lane"); + p.objects_filtering_params.invert_opposite_lane = + node->declare_parameter(obj_filter_ns + "invert_opposite_lane"); + p.objects_filtering_params.check_all_predicted_path = + node->declare_parameter(obj_filter_ns + "check_all_predicted_path"); + p.objects_filtering_params.use_all_predicted_path = + node->declare_parameter(obj_filter_ns + "use_all_predicted_path"); + p.objects_filtering_params.use_predicted_path_outside_lanelet = + node->declare_parameter(obj_filter_ns + "use_predicted_path_outside_lanelet"); + } + + // ObjectTypesToCheck + std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + { + p.objects_filtering_params.object_types_to_check.check_car = + node->declare_parameter(obj_types_ns + "check_car"); + p.objects_filtering_params.object_types_to_check.check_truck = + node->declare_parameter(obj_types_ns + "check_truck"); + p.objects_filtering_params.object_types_to_check.check_bus = + node->declare_parameter(obj_types_ns + "check_bus"); + p.objects_filtering_params.object_types_to_check.check_trailer = + node->declare_parameter(obj_types_ns + "check_trailer"); + p.objects_filtering_params.object_types_to_check.check_unknown = + node->declare_parameter(obj_types_ns + "check_unknown"); + p.objects_filtering_params.object_types_to_check.check_bicycle = + node->declare_parameter(obj_types_ns + "check_bicycle"); + p.objects_filtering_params.object_types_to_check.check_motorcycle = + node->declare_parameter(obj_types_ns + "check_motorcycle"); + p.objects_filtering_params.object_types_to_check.check_pedestrian = + node->declare_parameter(obj_types_ns + "check_pedestrian"); + } + + // ObjectLaneConfiguration + std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + { + p.objects_filtering_params.object_lane_configuration.check_current_lane = + node->declare_parameter(obj_lane_ns + "check_current_lane"); + p.objects_filtering_params.object_lane_configuration.check_right_lane = + node->declare_parameter(obj_lane_ns + "check_right_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_left_lane = + node->declare_parameter(obj_lane_ns + "check_left_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = + node->declare_parameter(obj_lane_ns + "check_shoulder_lane"); + p.objects_filtering_params.object_lane_configuration.check_other_lane = + node->declare_parameter(obj_lane_ns + "check_other_lane"); + } + + // SafetyCheckParams + std::string safety_check_ns = base_ns + "safety_check_params."; + { + p.safety_check_params.enable_safety_check = + node->declare_parameter(safety_check_ns + "enable_safety_check"); + p.safety_check_params.backward_path_length = + node->declare_parameter(safety_check_ns + "backward_path_length"); + p.safety_check_params.forward_path_length = + node->declare_parameter(safety_check_ns + "forward_path_length"); + p.safety_check_params.publish_debug_marker = + node->declare_parameter(safety_check_ns + "publish_debug_marker"); + } + + // RSSparams + std::string rss_ns = safety_check_ns + "rss_params."; + { + p.safety_check_params.rss_params.rear_vehicle_reaction_time = + node->declare_parameter(rss_ns + "rear_vehicle_reaction_time"); + p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = + node->declare_parameter(rss_ns + "rear_vehicle_safety_time_margin"); + p.safety_check_params.rss_params.lateral_distance_max_threshold = + node->declare_parameter(rss_ns + "lateral_distance_max_threshold"); + p.safety_check_params.rss_params.longitudinal_distance_min_threshold = + node->declare_parameter(rss_ns + "longitudinal_distance_min_threshold"); + p.safety_check_params.rss_params.longitudinal_velocity_delta_time = + node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); + } + // validation of parameters if (p.lateral_acceleration_sampling_num < 1) { RCLCPP_FATAL_STREAM( diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index abdd41f4935c5..a285224404f12 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -17,12 +17,13 @@ #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include #include #include -#include #include #include @@ -97,6 +98,16 @@ BehaviorModuleOutput StartPlannerModule::run() return plan(); } +void StartPlannerModule::processOnEntry() +{ + // Initialize safety checker + if (parameters_->safety_check_params.enable_safety_check) { + initializeSafetyCheckParameters(); + utils::start_goal_planner_common::initializeCollisionCheckDebugMap( + start_planner_data_.collision_check); + } +} + void StartPlannerModule::processOnExit() { resetPathCandidate(); @@ -108,7 +119,7 @@ bool StartPlannerModule::isExecutionRequested() const { // TODO(Sugahara): if required lateral shift distance is small, don't engage this module. // Execute when current pose is near route start pose - const Pose & start_pose = planner_data_->route_handler->getOriginalStartPose(); + const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); const Pose & current_pose = planner_data_->self_odometry->pose.pose; if ( tier4_autoware_utils::calcDistance2d(start_pose.position, current_pose.position) > @@ -117,7 +128,7 @@ bool StartPlannerModule::isExecutionRequested() const } // Check if ego arrives at goal - const Pose & goal_pose = planner_data_->route_handler->getGoalPose(); + const Pose goal_pose = planner_data_->route_handler->getGoalPose(); if ( tier4_autoware_utils::calcDistance2d(goal_pose.position, current_pose.position) < parameters_->th_arrived_distance) { @@ -134,41 +145,25 @@ bool StartPlannerModule::isExecutionRequested() const return false; } - // Create vehicle footprint - const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); - const auto vehicle_footprint = transformVector( - local_vehicle_footprint, - tier4_autoware_utils::pose2transform(planner_data_->self_odometry->pose.pose)); - - // Check if ego is not out of lanes - const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_->max_back_distance; - const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), - /*forward_only_in_route*/ true); - const auto pull_out_lanes = - start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); - auto lanes = current_lanes; - for (const auto & pull_out_lane : pull_out_lanes) { - auto it = std::find_if( - lanes.begin(), lanes.end(), [&pull_out_lane](const lanelet::ConstLanelet & lane) { - return lane.id() == pull_out_lane.id(); - }); - - if (it == lanes.end()) { - lanes.push_back(pull_out_lane); - } - } - - if (LaneDepartureChecker::isOutOfLane(lanes, vehicle_footprint)) { - return false; - } - return true; } bool StartPlannerModule::isExecutionReady() const { + if (!status_.is_safe_static_objects) { + return false; + } + + if (status_.pull_out_path.partial_paths.empty()) { + return true; + } + + if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) { + if (!isSafePath()) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); + return false; + } + } return true; } @@ -198,6 +193,7 @@ BehaviorModuleOutput StartPlannerModule::plan() getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } @@ -215,6 +211,7 @@ BehaviorModuleOutput StartPlannerModule::plan() getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } @@ -280,6 +277,15 @@ CandidateOutput StartPlannerModule::planCandidate() const return CandidateOutput{}; } +void StartPlannerModule::initializeSafetyCheckParameters() +{ + utils::start_goal_planner_common::updateEgoPredictedPathParams( + ego_predicted_path_params_, parameters_); + utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); + utils::start_goal_planner_common::updateObjectsFilteringParams( + objects_filtering_params_, parameters_); +} + PathWithLaneId StartPlannerModule::getFullPath() const { // combine partial pull out path @@ -311,6 +317,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() clearWaitingApproval(); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } @@ -321,6 +328,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() clearWaitingApproval(); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } @@ -407,8 +415,8 @@ PathWithLaneId StartPlannerModule::getCurrentPath() const } void StartPlannerModule::planWithPriority( - const std::vector & start_pose_candidates, const Pose & goal_pose, - const std::string search_priority) + const std::vector & start_pose_candidates, const Pose & refined_start_pose, + const Pose & goal_pose, const std::string search_priority) { // check if start pose candidates are valid if (start_pose_candidates.empty()) { @@ -416,12 +424,13 @@ void StartPlannerModule::planWithPriority( } const auto is_safe_with_pose_planner = [&](const size_t i, const auto & planner) { - // Set back_finished flag based on the current index - status_.back_finished = i == 0; - // Get the pull_out_start_pose for the current index const auto & pull_out_start_pose = start_pose_candidates.at(i); + // Set back_finished to true if the current start pose is same to refined_start_pose + status_.back_finished = + tier4_autoware_utils::calcDistance2d(pull_out_start_pose, refined_start_pose) < 0.01; + planner->setPlannerData(planner_data_); const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose); // not found safe path @@ -497,7 +506,6 @@ void StartPlannerModule::planWithPriority( for (const auto & p : order_priority) { if (is_safe_with_pose_planner(p.first, p.second)) { - const std::lock_guard lock(mutex_); return; } } @@ -566,10 +574,15 @@ std::vector StartPlannerModule::generateDrivableLanes( const PathWithLaneId & path) const { const auto path_road_lanes = getPathRoadLanes(path); - if (!path_road_lanes.empty()) { - return utils::generateDrivableLanesWithShoulderLanes( - getPathRoadLanes(path), status_.pull_out_lanes); + lanelet::ConstLanelets shoulder_lanes; + const auto & rh = planner_data_->route_handler; + std::copy_if( + status_.pull_out_lanes.begin(), status_.pull_out_lanes.end(), + std::back_inserter(shoulder_lanes), + [&rh](const auto & pull_out_lane) { return rh->isShoulderLanelet(pull_out_lane); }); + + return utils::generateDrivableLanesWithShoulderLanes(path_road_lanes, shoulder_lanes); } // if path_road_lanes is empty, use only pull_out_lanes as drivable lanes @@ -600,7 +613,7 @@ void StartPlannerModule::updatePullOutStatus() // skip updating if enough time has not passed for preventing chattering between back and // start_planner - if (!has_received_new_route && !last_pull_out_start_update_time_ && !status_.back_finished) { + if (!has_received_new_route) { if (!last_pull_out_start_update_time_) { last_pull_out_start_update_time_ = std::make_unique(clock_->now()); } @@ -615,9 +628,24 @@ void StartPlannerModule::updatePullOutStatus() const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & goal_pose = planner_data_->route_handler->getGoalPose(); + // refine start pose with pull out lanes. + // 1) backward driving is not allowed: use refined pose just as start pose. + // 2) backward driving is allowed: use refined pose to check if backward driving is needed. + const PathWithLaneId start_pose_candidates_path = calcStartPoseCandidatesBackwardPath(); + const auto refined_start_pose = calcLongitudinalOffsetPose( + start_pose_candidates_path.points, planner_data_->self_odometry->pose.pose.position, 0.0); + if (!refined_start_pose) return; + // search pull out start candidates backward - std::vector start_pose_candidates = searchPullOutStartPoses(); - planWithPriority(start_pose_candidates, goal_pose, parameters_->search_priority); + const std::vector start_pose_candidates = std::invoke([&]() -> std::vector { + if (parameters_->enable_back) { + return searchPullOutStartPoses(start_pose_candidates_path); + } + return {*refined_start_pose}; + }); + + planWithPriority( + start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); checkBackFinished(); if (!status_.back_finished) { @@ -627,20 +655,34 @@ void StartPlannerModule::updatePullOutStatus() } } -std::vector StartPlannerModule::searchPullOutStartPoses() +PathWithLaneId StartPlannerModule::calcStartPoseCandidatesBackwardPath() const { - std::vector pull_out_start_pose{}; - const Pose & current_pose = planner_data_->self_odometry->pose.pose; // get backward shoulder path const auto arc_position_pose = lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose); const double check_distance = parameters_->max_back_distance + 30.0; // buffer - auto backward_shoulder_path = planner_data_->route_handler->getCenterLinePath( + auto path = planner_data_->route_handler->getCenterLinePath( status_.pull_out_lanes, arc_position_pose.length - check_distance, arc_position_pose.length + check_distance); + // lateral shift to current_pose + const double distance_from_center_line = arc_position_pose.distance; + for (auto & p : path.points) { + p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0); + } + + return path; +} + +std::vector StartPlannerModule::searchPullOutStartPoses( + const PathWithLaneId & start_pose_candidates) const +{ + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + + std::vector pull_out_start_pose{}; + // filter pull out lanes stop objects const auto [pull_out_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( @@ -648,28 +690,19 @@ std::vector StartPlannerModule::searchPullOutStartPoses() const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_out_lane_objects, parameters_->th_moving_object_velocity); - // lateral shift to current_pose - const double distance_from_center_line = arc_position_pose.distance; - for (auto & p : backward_shoulder_path.points) { - p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0); - } - - // if backward driving is disable, just refine current pose to the lanes - if (!parameters_->enable_back) { - const auto refined_pose = - calcLongitudinalOffsetPose(backward_shoulder_path.points, current_pose.position, 0); - if (refined_pose) { - pull_out_start_pose.push_back(*refined_pose); - } - return pull_out_start_pose; - } + // Set the maximum backward distance less than the distance from the vehicle's base_link to the + // lane's rearmost point to prevent lane departure. + const double s_current = + lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose).length; + const double max_back_distance = std::clamp( + s_current - planner_data_->parameters.base_link2rear, 0.0, parameters_->max_back_distance); // check collision between footprint and object at the backed pose const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); - for (double back_distance = 0.0; back_distance <= parameters_->max_back_distance; + for (double back_distance = 0.0; back_distance <= max_back_distance; back_distance += parameters_->backward_search_resolution) { const auto backed_pose = calcLongitudinalOffsetPose( - backward_shoulder_path.points, current_pose.position, -back_distance); + start_pose_candidates.points, current_pose.position, -back_distance); if (!backed_pose) { continue; } @@ -677,10 +710,10 @@ std::vector StartPlannerModule::searchPullOutStartPoses() // check the back pose is near the lane end const double length_to_backed_pose = lanelet::utils::getArcCoordinates(status_.pull_out_lanes, *backed_pose).length; - double length_to_lane_end = 0.0; - for (const auto & lane : status_.pull_out_lanes) { - length_to_lane_end += lanelet::utils::getLaneletLength2d(lane); - } + + const double length_to_lane_end = std::accumulate( + std::begin(status_.pull_out_lanes), std::end(status_.pull_out_lanes), 0.0, + [](double acc, const auto & lane) { return acc + lanelet::utils::getLaneletLength2d(lane); }); const double distance_from_lane_end = length_to_lane_end - length_to_backed_pose; if (distance_from_lane_end < parameters_->ignore_distance_from_lane_end) { RCLCPP_WARN_THROTTLE( @@ -923,6 +956,85 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const return turn_signal; } +bool StartPlannerModule::isSafePath() const +{ + // TODO(Sugahara): should safety check for backward path + + const auto pull_out_path = getCurrentPath(); + const auto & current_pose = planner_data_->self_odometry->pose.pose; + const double current_velocity = std::hypot( + planner_data_->self_odometry->twist.twist.linear.x, + planner_data_->self_odometry->twist.twist.linear.y); + const auto & dynamic_object = planner_data_->dynamic_object; + const auto & route_handler = planner_data_->route_handler; + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + + // for ego predicted path + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_out_path.points); + const std::pair terminal_velocity_and_accel = + utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( + status_.pull_out_path.pairs_terminal_velocity_and_accel, status_.current_path_idx); + RCLCPP_DEBUG( + getLogger(), "pairs_terminal_velocity_and_accel for start_planner: %f, %f", + terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); + RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.current_path_idx); + utils::start_goal_planner_common::updatePathProperty( + ego_predicted_path_params_, terminal_velocity_and_accel); + const auto ego_predicted_path = + behavior_path_planner::utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params_, pull_out_path.points, current_pose, current_velocity, + ego_seg_idx); + + // filtering objects with velocity, position and class + const auto & filtered_objects = utils::path_safety_checker::filterObjects( + dynamic_object, route_handler, current_lanes, current_pose.position, objects_filtering_params_); + + // filtering objects based on the current position's lane + const auto & target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + current_lanes, route_handler, filtered_objects, objects_filtering_params_); + + const double hysteresis_factor = + status_.is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate; + + utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( + start_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); + + bool is_safe_dynamic_objects = true; + // Check for collisions with each predicted path of the object + for (const auto & object : target_objects_on_lane.on_current_lane) { + auto current_debug_data = marker_utils::createObjectDebug(object); + + bool is_safe_dynamic_object = true; + + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + object, objects_filtering_params_->check_all_predicted_path); + + // If a collision is detected, mark the object as unsafe and break the loop + for (const auto & obj_path : obj_predicted_paths) { + if (!utils::path_safety_checker::checkCollision( + pull_out_path, ego_predicted_path, object, obj_path, planner_data_->parameters, + safety_check_params_->rss_params, hysteresis_factor, current_debug_data.second)) { + marker_utils::updateCollisionCheckDebugMap( + start_planner_data_.collision_check, current_debug_data, false); + is_safe_dynamic_objects = false; + is_safe_dynamic_object = false; + break; + } + } + if (is_safe_dynamic_object) { + marker_utils::updateCollisionCheckDebugMap( + start_planner_data_.collision_check, current_debug_data, is_safe_dynamic_object); + } + } + + return is_safe_dynamic_objects; +} + bool StartPlannerModule::IsGoalBehindOfEgoInSameRouteSegment() const { const auto & rh = planner_data_->route_handler; @@ -1041,8 +1153,13 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons void StartPlannerModule::setDebugData() const { + using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; + using marker_utils::createPredictedPathMarkerArray; + using marker_utils::showPolygon; + using marker_utils::showPredictedPath; + using marker_utils::showSafetyCheckInfo; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; @@ -1060,6 +1177,36 @@ void StartPlannerModule::setDebugData() const add(createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3)); add(createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3)); add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); + if (start_planner_data_.ego_predicted_path.size() > 0) { + const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( + start_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); + add(createPredictedPathMarkerArray( + ego_predicted_path, vehicle_info_, "ego_predicted_path", 0, 0.0, 0.5, 0.9)); + } + + if (start_planner_data_.filtered_objects.objects.size() > 0) { + add(createObjectsMarkerArray( + start_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + } + + // safety check + { + add(showSafetyCheckInfo(start_planner_data_.collision_check, "object_debug_info")); + add(showPredictedPath(start_planner_data_.collision_check, "ego_predicted_path")); + add(showPolygon(start_planner_data_.collision_check, "ego_and_target_polygon_relation")); + } + + if (start_planner_data_.ego_predicted_path.size() > 0) { + const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( + start_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); + add(createPredictedPathMarkerArray( + ego_predicted_path, vehicle_info_, "ego_predicted_path", 0, 0.0, 0.5, 0.9)); + } + + if (start_planner_data_.filtered_objects.objects.size() > 0) { + add(createObjectsMarkerArray( + start_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + } // Visualize planner type text const auto header = planner_data_->route_handler->getRouteHeader(); diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index fa661aef06681..65e3e8343f49c 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -18,9 +18,13 @@ #include #include +#include #include +#include #include -#include +#include +#include +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index b60abbddf6632..4afe5381daa64 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -17,10 +17,14 @@ #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/avoidance/utils.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include -#include +#include +#include +#include #include diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 0fcb0acfcb000..52be0ac5a81ef 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -22,7 +22,10 @@ #include #include +#include +#include #include +#include #include diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index 27dbfb532c1fc..5b054de43ef74 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -17,12 +17,13 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" #include #include #include -#include #ifdef ROS_DISTRO_GALACTIC #include @@ -447,7 +448,6 @@ std::vector GeometricParallelParking::planOneTrial( if (std::abs(end_pose_offset) > 0) { PathPointWithLaneId straight_point{}; straight_point.point.pose = goal_pose; - // setLaneIds(straight_point); path_turn_right.points.push_back(straight_point); } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp index 9656094f5a478..5bb4e924b6d89 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" +#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" #include #include @@ -63,8 +64,9 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); - const auto pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); if (road_lanes.empty() || pull_over_lanes.empty()) { return {}; } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp index 7760b9b57d8ab..f350262cf4b80 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include @@ -47,13 +48,13 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); - const auto shoulder_lanes = - goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); - if (road_lanes.empty() || shoulder_lanes.empty()) { + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); + if (road_lanes.empty() || pull_over_lanes.empty()) { return {}; } - auto lanes = road_lanes; - lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); + const auto lanes = utils::combineLanelets(road_lanes, pull_over_lanes); const auto & p = parallel_parking_parameters_; const double max_steer_angle = @@ -62,7 +63,7 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) planner_.setPlannerData(planner_data_); const bool found_valid_path = - planner_.planPullOver(goal_pose, road_lanes, shoulder_lanes, is_forward_); + planner_.planPullOver(goal_pose, road_lanes, pull_over_lanes, is_forward_); if (!found_valid_path) { return {}; } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index e12ee576a5d4b..eb4a2eb1e7cbc 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -18,7 +18,9 @@ #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "lanelet2_extension/utility/utilities.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" +#include #include #include @@ -57,8 +59,9 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) const double base_link2front = planner_data_->parameters.base_link2front; const double base_link2rear = planner_data_->parameters.base_link2rear; - const auto pull_over_lanes = - goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); auto lanes = utils::getExtendedCurrentLanes( planner_data_, backward_length, forward_length, /*forward_only_in_route*/ false); @@ -72,10 +75,6 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), parameters_.goal_search_interval); - const auto [shoulder_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes); - std::vector original_search_poses{}; // for search area visualizing size_t goal_id = 0; for (const auto & p : center_line_path.points) { @@ -156,15 +155,18 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const continue; } - // check margin with pull over lane objects - const auto pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); - const auto [shoulder_lane_objects, others] = + // check longitudinal margin with pull over lane objects + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); + const auto [pull_over_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( *(planner_data_->dynamic_object), pull_over_lanes); + const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_over_lane_objects, parameters_.th_moving_object_velocity); constexpr bool filter_inside = true; const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance( - goal_pose, planner_data_->parameters.vehicle_width, shoulder_lane_objects, + goal_pose, planner_data_->parameters.vehicle_width, pull_over_lane_stop_objects, parameters_.object_recognition_collision_check_margin, filter_inside); if (checkCollisionWithLongitudinalDistance(goal_pose, target_objects)) { goal_candidate.is_safe = false; @@ -177,7 +179,7 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const bool GoalSearcher::checkCollision(const Pose & pose) const { - if (parameters_.use_occupancy_grid) { + if (parameters_.use_occupancy_grid_for_goal_search) { const Pose pose_grid_coords = global2local(occupancy_grid_map_->getMap(), pose); const auto idx = pose2index( occupancy_grid_map_->getMap(), pose_grid_coords, occupancy_grid_map_->getParam().theta_size); @@ -188,8 +190,16 @@ bool GoalSearcher::checkCollision(const Pose & pose) const } if (parameters_.use_object_recognition) { + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); + const auto [pull_over_lane_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_over_lanes); + const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_over_lane_objects, parameters_.th_moving_object_velocity); if (utils::checkCollisionBetweenFootprintAndObjects( - vehicle_footprint_, pose, *(planner_data_->dynamic_object), + vehicle_footprint_, pose, pull_over_lane_stop_objects, parameters_.object_recognition_collision_check_margin)) { return true; } @@ -200,7 +210,9 @@ bool GoalSearcher::checkCollision(const Pose & pose) const bool GoalSearcher::checkCollisionWithLongitudinalDistance( const Pose & ego_pose, const PredictedObjects & dynamic_objects) const { - if (parameters_.use_occupancy_grid && parameters_.use_occupancy_grid_for_longitudinal_margin) { + if ( + parameters_.use_occupancy_grid_for_goal_search && + parameters_.use_occupancy_grid_for_goal_longitudinal_margin) { constexpr bool check_out_of_range = false; const double offset = std::max( parameters_.longitudinal_margin - parameters_.occupancy_grid_collision_check_margin, 0.0); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index c5ac14d4d9727..d6a3746187b11 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -16,6 +16,8 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include @@ -49,16 +51,16 @@ boost::optional ShiftPullOver::plan(const Pose & goal_pose) const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, backward_search_length, forward_search_length, /*forward_only_in_route*/ false); - const auto shoulder_lanes = - goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); - if (road_lanes.empty() || shoulder_lanes.empty()) { + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, backward_search_length, forward_search_length); + if (road_lanes.empty() || pull_over_lanes.empty()) { return {}; } // find safe one from paths with different jerk for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { const auto pull_over_path = - generatePullOverPath(road_lanes, shoulder_lanes, goal_pose, lateral_jerk); + generatePullOverPath(road_lanes, pull_over_lanes, goal_pose, lateral_jerk); if (!pull_over_path) continue; return *pull_over_path; } @@ -170,6 +172,9 @@ boost::optional ShiftPullOver::generatePullOverPath( shifted_path.path.points.push_back(p); } + shifted_path.path = + utils::start_goal_planner_common::removeInverseOrderPathPoints(shifted_path.path); + // set the same z as the goal for (auto & p : shifted_path.path.points) { p.point.pose.position.z = goal_pose.position.z; diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp index 0d58a54431be0..aa3380c2b99df 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -55,36 +55,37 @@ PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWith return path; } -lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler, const bool left_side) +lanelet::ConstLanelets getPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance) { const Pose goal_pose = route_handler.getOriginalGoalPose(); + // Buffer to get enough lanes in front of the goal, need much longer than the pull over distance. + // In the case of loop lanes, it may not be possible to extend the lane forward. + // todo(kosuek55): automatically calculates this distance. + const double backward_distance_with_buffer = backward_distance + 100; + lanelet::ConstLanelet target_shoulder_lane{}; if (route_handler::RouteHandler::getPullOverTarget( route_handler.getShoulderLanelets(), goal_pose, &target_shoulder_lane)) { // pull over on shoulder lane - return route_handler.getShoulderLaneletSequence(target_shoulder_lane, goal_pose); + return route_handler.getShoulderLaneletSequence( + target_shoulder_lane, goal_pose, backward_distance_with_buffer, forward_distance); } - // pull over on road lane lanelet::ConstLanelet closest_lane{}; route_handler.getClosestLaneletWithinRoute(goal_pose, &closest_lane); - lanelet::ConstLanelet outermost_lane{}; if (left_side) { - outermost_lane = route_handler.getMostLeftLanelet(closest_lane); + outermost_lane = route_handler.getMostLeftLanelet(closest_lane, false, true); } else { - outermost_lane = route_handler.getMostRightLanelet(closest_lane); - } - - lanelet::ConstLanelet outermost_shoulder_lane; - if (route_handler.getLeftShoulderLanelet(outermost_lane, &outermost_shoulder_lane)) { - return route_handler.getShoulderLaneletSequence(outermost_shoulder_lane, goal_pose); + outermost_lane = route_handler.getMostRightLanelet(closest_lane, false, true); } - const bool dist = std::numeric_limits::max(); constexpr bool only_route_lanes = false; - return route_handler.getLaneletSequence(outermost_lane, dist, dist, only_route_lanes); + return route_handler.getLaneletSequence( + outermost_lane, backward_distance_with_buffer, forward_distance, only_route_lanes); } PredictedObjects filterObjectsByLateralDistance( diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index f3a1260eecadc..ffd5116a5d2dd 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -25,8 +25,11 @@ #include #include #include +#include +#include #include #include +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index d244d08abf99b..0f87c68e38289 100644 --- a/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -15,7 +15,7 @@ #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include -#include +#include #include diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index 23016b01cb9ab..7205b273ff281 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -16,6 +16,10 @@ #include "behavior_path_planner/utils/utils.hpp" +#include +#include +#include + namespace behavior_path_planner::utils::path_safety_checker { 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 1838dc6bdc07b..dddcc672e7107 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 @@ -16,8 +16,10 @@ #include "behavior_path_planner/marker_utils/utils.hpp" #include "interpolation/linear_interpolation.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" namespace behavior_path_planner::utils::path_safety_checker { diff --git a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp index 9ca083af35c92..44ba6bd364bc4 100644 --- a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index c4bfb62276d42..112c7e49c7c86 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -15,11 +15,13 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include #include -#include +#include +#include #include diff --git a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp index f29711c82b3ce..bd695055b2079 100644 --- a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp +++ b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp @@ -14,6 +14,8 @@ #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" +#include + namespace behavior_path_planner::utils::start_goal_planner_common { @@ -131,8 +133,30 @@ void updatePathProperty( std::shared_ptr & ego_predicted_path_params, const std::pair & pairs_terminal_velocity_and_accel) { + // If acceleration is close to 0, the ego predicted path will be too short, so a minimum value is + // necessary to ensure a reasonable path length. + // TODO(Sugahara): define them as parameter + const double min_accel_for_ego_predicted_path = 1.0; + const double acceleration = + std::max(pairs_terminal_velocity_and_accel.second, min_accel_for_ego_predicted_path); + ego_predicted_path_params->target_velocity = pairs_terminal_velocity_and_accel.first; - ego_predicted_path_params->acceleration = pairs_terminal_velocity_and_accel.second; + ego_predicted_path_params->acceleration = acceleration; +} + +void initializeCollisionCheckDebugMap(CollisionCheckDebugMap & collision_check_debug_map) +{ + collision_check_debug_map.clear(); +} + +void updateSafetyCheckTargetObjectsData( + StartGoalPlannerData & data, const PredictedObjects & filtered_objects, + const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) +{ + data.filtered_objects = filtered_objects; + data.target_objects_on_lane = target_objects_on_lane; + data.ego_predicted_path = ego_predicted_path; } std::pair getPairsTerminalVelocityAndAccel( @@ -145,4 +169,19 @@ std::pair getPairsTerminalVelocityAndAccel( return pairs_terminal_velocity_and_accel.at(current_path_idx); } +PathWithLaneId removeInverseOrderPathPoints(const PathWithLaneId & path) +{ + PathWithLaneId fixed_path; + fixed_path.header = path.header; + fixed_path.points.push_back(path.points.at(0)); + for (size_t i = 1; i < path.points.size(); i++) { + const auto p1 = path.points.at(i - 1).point.pose; + const auto p2 = path.points.at(i).point.pose; + if (tier4_autoware_utils::isDrivingForward(p1, p2)) { + fixed_path.points.push_back(path.points.at(i)); + } + } + return fixed_path; +} + } // namespace behavior_path_planner::utils::start_goal_planner_common diff --git a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp index ba34901d9df6a..36fb6381ac967 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp @@ -44,7 +44,7 @@ FreespacePullOut::FreespacePullOut( } } -boost::optional FreespacePullOut::plan(const Pose start_pose, const Pose end_pose) +boost::optional FreespacePullOut::plan(const Pose & start_pose, const Pose & end_pose) { const auto & route_handler = planner_data_->route_handler; const double backward_path_length = planner_data_->parameters.backward_path_length; @@ -86,7 +86,7 @@ boost::optional FreespacePullOut::plan(const Pose start_pose, const } // push back generate road lane path between end pose and goal pose to last path - const auto & goal_pose = route_handler->getGoalPose(); + const Pose goal_pose = route_handler->getGoalPose(); constexpr double offset_from_end_pose = 1.0; const auto arc_position_end = lanelet::utils::getArcCoordinates(road_lanes, end_pose); const double s_start = std::max(arc_position_end.length + offset_from_end_pose, 0.0); diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 29abdb08c5c55..0d75391f9d4a6 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/utils/utils.hpp" #include +#include using lanelet::utils::getArcCoordinates; using motion_utils::findNearestIndex; @@ -36,7 +37,7 @@ GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const StartPlannerParame planner_.setParameters(parallel_parking_parameters_); } -boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_pose) +boost::optional GeometricPullOut::plan(const Pose & start_pose, const Pose & goal_pose) { PullOutPath output; @@ -47,16 +48,6 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p planner_data_, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length); - auto lanes = road_lanes; - for (const auto & pull_out_lane : pull_out_lanes) { - auto it = std::find_if( - lanes.begin(), lanes.end(), [&pull_out_lane](const lanelet::ConstLanelet & lane) { - return lane.id() == pull_out_lane.id(); - }); - if (it == lanes.end()) { - lanes.push_back(pull_out_lane); - } - } planner_.setTurningRadius( planner_data_->parameters, parallel_parking_parameters_.pull_out_max_steer_angle); diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index 7352821e140be..6225ce598afe3 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -16,8 +16,10 @@ #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include @@ -40,7 +42,7 @@ ShiftPullOut::ShiftPullOut( { } -boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) +boost::optional ShiftPullOut::plan(const Pose & start_pose, const Pose & goal_pose) { const auto & route_handler = planner_data_->route_handler; const auto & common_parameters = planner_data_->parameters; @@ -96,17 +98,48 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) shift_path.points.begin() + collision_check_end_idx + 1); } - // check lane departure + // extract shoulder lanes from pull out lanes + lanelet::ConstLanelets shoulder_lanes; + std::copy_if( + pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes), + [&route_handler](const auto & pull_out_lane) { + return route_handler->isShoulderLanelet(pull_out_lane); + }); const auto drivable_lanes = - utils::generateDrivableLanesWithShoulderLanes(road_lanes, pull_out_lanes); + utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); const auto & dp = planner_data_->drivable_area_expansion_parameters; - const auto expanded_lanes = utils::expandLanelets( + const auto expanded_lanes = utils::transformToLanelets(utils::expandLanelets( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, - dp.drivable_area_types_to_skip); + dp.drivable_area_types_to_skip)); + + // crop backward path + // removes points which are out of lanes up to the start pose. + // this ensures that the backward_path stays within the drivable area when starting from a + // narrow place. + const size_t start_segment_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + shift_path.points, start_pose, common_parameters.ego_nearest_dist_threshold, + common_parameters.ego_nearest_yaw_threshold); + PathWithLaneId cropped_path{}; + for (size_t i = 0; i < shift_path.points.size(); ++i) { + const Pose pose = shift_path.points.at(i).point.pose; + const auto transformed_vehicle_footprint = + transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(pose)); + const bool is_out_of_lane = + LaneDepartureChecker::isOutOfLane(expanded_lanes, transformed_vehicle_footprint); + if (i <= start_segment_idx) { + if (!is_out_of_lane) { + cropped_path.points.push_back(shift_path.points.at(i)); + } + } else { + cropped_path.points.push_back(shift_path.points.at(i)); + } + } + shift_path.points = cropped_path.points; + + // check lane departure if ( parameters_.check_shift_path_lane_departure && - lane_departure_checker_->checkPathWillLeaveLane( - utils::transformToLanelets(expanded_lanes), path_start_to_end)) { + lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_start_to_end)) { continue; } @@ -266,6 +299,9 @@ std::vector ShiftPullOut::calcPullOutPaths( continue; } + shifted_path.path = + utils::start_goal_planner_common::removeInverseOrderPathPoints(shifted_path.path); + // set velocity const size_t pull_out_end_idx = findNearestIndex(shifted_path.path.points, shift_end_pose_ptr->position); diff --git a/planning/behavior_path_planner/src/utils/start_planner/util.cpp b/planning/behavior_path_planner/src/utils/start_planner/util.cpp index 9ee0491ca017e..f5b4c9b979ee3 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/util.cpp @@ -20,6 +20,7 @@ #include "behavior_path_planner/utils/utils.hpp" #include +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index da23d818cf59a..d6af88f39a8d2 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -20,6 +20,9 @@ #include #include #include +#include +#include +#include #include "autoware_auto_perception_msgs/msg/predicted_object.hpp" #include "autoware_auto_perception_msgs/msg/predicted_path.hpp" @@ -2848,7 +2851,7 @@ BehaviorModuleOutput getReferencePath( // clip backward length // NOTE: In order to keep backward_path_length at least, resampling interval is added to the // backward. - const size_t current_seg_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( reference_path.points, no_shift_pose, p.ego_nearest_dist_threshold, p.ego_nearest_yaw_threshold); reference_path.points = motion_utils::cropPoints( diff --git a/planning/behavior_path_planner/test/test_lane_change_utils.cpp b/planning/behavior_path_planner/test/test_lane_change_utils.cpp index 3a809509641f4..ad64a7ae3feb5 100644 --- a/planning/behavior_path_planner/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_planner/test/test_lane_change_utils.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include +#include #include #include diff --git a/planning/behavior_path_planner/test/test_safety_check.cpp b/planning/behavior_path_planner/test/test_safety_check.cpp index 9dfcef487cc81..39e831bd5565b 100644 --- a/planning/behavior_path_planner/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/test/test_safety_check.cpp @@ -15,7 +15,8 @@ #include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include #include diff --git a/planning/behavior_path_planner/test/test_turn_signal.cpp b/planning/behavior_path_planner/test/test_turn_signal.cpp index 8c6f8de7ee6a0..4917443064e78 100644 --- a/planning/behavior_path_planner/test/test_turn_signal.cpp +++ b/planning/behavior_path_planner/test/test_turn_signal.cpp @@ -12,8 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. #include "behavior_path_planner/turn_signal_decider.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" + +#include +#include +#include #include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include #include #include diff --git a/planning/behavior_path_planner/test/test_utils.cpp b/planning/behavior_path_planner/test/test_utils.cpp index b9e2727ebaa13..1452e45980793 100644 --- a/planning/behavior_path_planner/test/test_utils.cpp +++ b/planning/behavior_path_planner/test/test_utils.cpp @@ -16,6 +16,7 @@ #include "lanelet2_core/Attribute.h" #include "lanelet2_core/geometry/Point.h" #include "lanelet2_core/primitives/Lanelet.h" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include diff --git a/planning/behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp index dbd4764585d81..1652d9d56a8a0 100644 --- a/planning/behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/debug.cpp @@ -16,7 +16,8 @@ #include #include -#include +#include +#include #include 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/debug.cpp b/planning/behavior_velocity_crosswalk_module/src/debug.cpp index eae7491872cfd..d6a2a3cb185a1 100644 --- a/planning/behavior_velocity_crosswalk_module/src/debug.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/debug.cpp @@ -15,8 +15,9 @@ #include "scene_crosswalk.hpp" #include -#include -#include +#include +#include +#include #include diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 6d2a3110cbdb7..ab84caa782d1c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -18,8 +18,11 @@ #include #include #include +#include #include -#include +#include +#include +#include #include #include @@ -938,28 +941,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; } } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index e0851366d7f71..11e2d600c687b 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -22,7 +22,8 @@ #include #include #include -#include +#include +#include #include #include @@ -50,8 +51,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_crosswalk_module/src/util.cpp b/planning/behavior_velocity_crosswalk_module/src/util.cpp index 72c091cd8bc85..6fabc4c201687 100644 --- a/planning/behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.cpp @@ -15,7 +15,9 @@ #include "behavior_velocity_crosswalk_module/util.hpp" #include -#include +#include +#include +#include #include diff --git a/planning/behavior_velocity_detection_area_module/src/debug.cpp b/planning/behavior_velocity_detection_area_module/src/debug.cpp index 60479d5adfd88..b3d3d5c781ff8 100644 --- a/planning/behavior_velocity_detection_area_module/src/debug.cpp +++ b/planning/behavior_velocity_detection_area_module/src/debug.cpp @@ -16,7 +16,8 @@ #include #include -#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index f22e9d788fc65..879b26209a05e 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/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index ae8eee39d6556..dfaa10ec9d86f 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -17,7 +17,8 @@ #include #include -#include +#include +#include #include #include diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 0dfadf2f00d7a..5f9d838b7c220 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -124,7 +125,7 @@ template void prepareRTCByDecisionResult( const T & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, - double * occlusion_distance, bool * occlusion_first_stop_required) + double * occlusion_distance) { static_assert("Unsupported type passed to prepareRTCByDecisionResult"); return; @@ -135,8 +136,7 @@ void prepareRTCByDecisionResult( [[maybe_unused]] const IntersectionModule::Indecisive & result, [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, - [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) { return; } @@ -145,17 +145,16 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::StuckStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "StuckStop"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto stop_line_idx = result.stop_line_idx; + const auto closest_idx = result.closest_idx; + const auto stop_line_idx = result.stuck_stop_line_idx; *default_safety = false; *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); *occlusion_safety = true; - if (!result.is_detection_area_empty) { - const auto occlusion_stop_line_idx = result.stop_lines.occlusion_peeking_stop_line; + if (result.occlusion_stop_line_idx) { + const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx.value(); *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); } @@ -166,15 +165,15 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::NonOccludedCollisionStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "NonOccludedCollisionStop"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto stop_line_idx = result.stop_line_idx; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; *default_safety = false; - *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); - const auto occlusion_stop_line = result.stop_lines.occlusion_peeking_stop_line; + *default_distance = + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); + const auto occlusion_stop_line = result.occlusion_stop_line_idx; *occlusion_safety = true; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line); @@ -185,11 +184,10 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::FirstWaitBeforeOcclusion & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "FirstWaitBeforeOcclusion"); - const auto closest_idx = result.stop_lines.closest_idx; + const auto closest_idx = result.closest_idx; const auto first_stop_line_idx = result.first_stop_line_idx; const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = false; @@ -198,7 +196,6 @@ void prepareRTCByDecisionResult( *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); - *occlusion_first_stop_required = true; return; } @@ -206,18 +203,18 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::PeekingTowardOcclusion & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "PeekingTowardOcclusion"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto stop_line_idx = result.stop_line_idx; - *occlusion_safety = result.is_actually_occlusion_cleared; - *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); - const auto default_stop_line_idx = result.stop_lines.default_stop_line; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; + const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = true; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, default_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); + *occlusion_safety = result.is_actually_occlusion_cleared; + *occlusion_distance = + motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); return; } @@ -225,15 +222,15 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::OccludedCollisionStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "OccludedCollisionStop"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto stop_line_idx = result.stop_line_idx; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = false; - *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); + *default_distance = + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); @@ -244,16 +241,15 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::Safe & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "Safe"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto default_stop_line_idx = result.stop_lines.default_stop_line; - const auto occlusion_stop_line_idx = result.stop_lines.occlusion_peeking_stop_line; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; + const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = true; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, default_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); *occlusion_safety = true; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); @@ -264,16 +260,15 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::TrafficLightArrowSolidOn & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "TrafficLightArrowSolidOn"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto default_stop_line_idx = result.stop_lines.default_stop_line; - const auto occlusion_stop_line_idx = result.stop_lines.occlusion_peeking_stop_line; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; + const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = !result.collision_detected; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, default_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); *occlusion_safety = true; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); @@ -290,11 +285,13 @@ void IntersectionModule::prepareRTCStatus( VisitorSwitch{[&](const auto & decision) { prepareRTCByDecisionResult( decision, path, &default_safety, &default_distance, &occlusion_safety_, - &occlusion_stop_distance_, &occlusion_first_stop_required_); + &occlusion_stop_distance_); }}, decision_result); setSafe(default_safety); setDistance(default_distance); + occlusion_first_stop_required_ = + std::holds_alternative(decision_result); } template @@ -335,9 +332,10 @@ void reactRTCApprovalByDecisionResult( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), "StuckStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); + const auto closest_idx = decision_result.closest_idx; if (!rtc_default_approved) { // use default_rtc uuid for stuck vehicle detection - const auto stop_line_idx = decision_result.stop_line_idx; + const auto stop_line_idx = decision_result.stuck_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -347,14 +345,14 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } if ( - !rtc_occlusion_approved && !decision_result.is_detection_area_empty && + !rtc_occlusion_approved && decision_result.occlusion_stop_line_idx && planner_param.occlusion.enable) { - const auto occlusion_stop_line_idx = decision_result.stop_lines.occlusion_peeking_stop_line; + const auto occlusion_stop_line_idx = decision_result.occlusion_stop_line_idx.value(); planning_utils::setVelocityFromIndex(occlusion_stop_line_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(occlusion_stop_line_idx, baselink2front, *path); @@ -363,7 +361,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(occlusion_stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(closest_idx).point.pose, path->points.at(occlusion_stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -383,7 +381,7 @@ void reactRTCApprovalByDecisionResult( "NonOccludedCollisionStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.stop_line_idx; + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -392,12 +390,12 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.stop_lines.occlusion_peeking_stop_line; + const auto stop_line_idx = decision_result.occlusion_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -406,7 +404,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -435,14 +433,14 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { if (planner_param.occlusion.enable_creeping) { const size_t occlusion_peeking_stop_line = decision_result.occlusion_stop_line_idx; - const size_t closest_idx = decision_result.stop_lines.closest_idx; + const size_t closest_idx = decision_result.closest_idx; for (size_t i = closest_idx; i < occlusion_peeking_stop_line; i++) { planning_utils::setVelocityFromIndex( i, planner_param.occlusion.occlusion_creep_velocity, path); @@ -457,7 +455,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -479,10 +477,9 @@ void reactRTCApprovalByDecisionResult( // NOTE: creep_velocity should be inserted first at closest_idx if !rtc_default_approved if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const size_t occlusion_peeking_stop_line = - decision_result.stop_lines.occlusion_peeking_stop_line; + const size_t occlusion_peeking_stop_line = decision_result.occlusion_stop_line_idx; if (planner_param.occlusion.enable_creeping) { - const size_t closest_idx = decision_result.stop_lines.closest_idx; + const size_t closest_idx = decision_result.closest_idx; for (size_t i = closest_idx; i < occlusion_peeking_stop_line; i++) { planning_utils::setVelocityFromIndex( i, planner_param.occlusion.occlusion_creep_velocity, path); @@ -496,12 +493,12 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(occlusion_peeking_stop_line).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(occlusion_peeking_stop_line).point.pose, VelocityFactor::INTERSECTION); } } - if (!rtc_default_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.stop_lines.default_stop_line; + if (!rtc_default_approved) { + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -510,7 +507,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -530,7 +527,7 @@ void reactRTCApprovalByDecisionResult( "OccludedCollisionStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.stop_line_idx; + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -539,7 +536,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -553,7 +550,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -572,7 +569,7 @@ void reactRTCApprovalByDecisionResult( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), "Safe, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.stop_lines.default_stop_line; + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -581,12 +578,12 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.stop_lines.occlusion_peeking_stop_line; + const auto stop_line_idx = decision_result.occlusion_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -595,7 +592,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -615,7 +612,7 @@ void reactRTCApprovalByDecisionResult( "TrafficLightArrowSolidOn, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.stop_lines.default_stop_line; + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -624,7 +621,21 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, + path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + } + } + if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + const auto stop_line_idx = decision_result.occlusion_stop_line_idx; + planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + debug_data->occlusion_stop_wall_pose = + planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + { + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor->set( + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -687,6 +698,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // calculate the const auto decision_result = modifyPathVelocityDetail(path, stop_reason); + prev_decision_result_ = decision_result; const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " + formatDecisionResult(decision_result); @@ -723,13 +735,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); if (!interpolated_path_info_opt) { RCLCPP_DEBUG(logger_, "splineInterpolate failed"); - is_peeking_ = false; return IntersectionModule::Indecisive{}; } const auto & interpolated_path_info = interpolated_path_info_opt.value(); if (!interpolated_path_info.lane_id_interval) { RCLCPP_WARN(logger_, "Path has no interval on intersection lane %ld", lane_id_); - is_peeking_ = false; return IntersectionModule::Indecisive{}; } @@ -748,43 +758,44 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( intersection_lanelets_.value().update(tl_arrow_solid_on, interpolated_path_info); const auto & conflicting_lanelets = intersection_lanelets_.value().conflicting(); - const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area(); - if (conflicting_lanelets.empty() || !first_conflicting_area) { - is_peeking_ = false; + const auto & first_conflicting_area_opt = intersection_lanelets_.value().first_conflicting_area(); + if (conflicting_lanelets.empty() || !first_conflicting_area_opt) { + RCLCPP_DEBUG(logger_, "conflicting area is empty"); return IntersectionModule::Indecisive{}; } + const auto first_conflicting_area = first_conflicting_area_opt.value(); - const auto & first_attention_area = intersection_lanelets_.value().first_attention_area(); + const auto & first_attention_area_opt = intersection_lanelets_.value().first_attention_area(); const auto & dummy_first_attention_area = - first_attention_area ? first_attention_area.value() : first_conflicting_area.value(); + first_attention_area_opt ? first_attention_area_opt.value() : first_conflicting_area; const auto intersection_stop_lines_opt = util::generateIntersectionStopLines( - first_conflicting_area.value(), dummy_first_attention_area, planner_data_, - interpolated_path_info, planner_param_.stuck_vehicle.use_stuck_stopline, - planner_param_.common.stop_line_margin, planner_param_.occlusion.peeking_offset, path); + first_conflicting_area, dummy_first_attention_area, planner_data_, interpolated_path_info, + planner_param_.stuck_vehicle.use_stuck_stopline, planner_param_.common.stop_line_margin, + planner_param_.occlusion.peeking_offset, path); if (!intersection_stop_lines_opt) { RCLCPP_DEBUG(logger_, "failed to generate intersection_stop_lines"); - is_peeking_ = false; return IntersectionModule::Indecisive{}; } const auto & intersection_stop_lines = intersection_stop_lines_opt.value(); const auto - [closest_idx, stuck_stop_line_idx, default_stop_line_idx, occlusion_peeking_stop_line_idx, - pass_judge_line_idx] = intersection_stop_lines; + [closest_idx, stuck_stop_line_idx_opt, default_stop_line_idx_opt, + occlusion_peeking_stop_line_idx_opt, pass_judge_line_idx] = intersection_stop_lines; const auto & conflicting_area = intersection_lanelets_.value().conflicting_area(); const auto path_lanelets_opt = util::generatePathLanelets( - lanelets_on_path, interpolated_path_info, associative_ids_, first_conflicting_area.value(), - conflicting_area, closest_idx, planner_data_->vehicle_info_.vehicle_width_m); + lanelets_on_path, interpolated_path_info, associative_ids_, first_conflicting_area, + conflicting_area, first_attention_area_opt, intersection_lanelets_.value().attention_area(), + closest_idx, planner_data_->vehicle_info_.vehicle_width_m); if (!path_lanelets_opt.has_value()) { RCLCPP_DEBUG(logger_, "failed to generate PathLanelets"); return IntersectionModule::Indecisive{}; } const auto path_lanelets = path_lanelets_opt.value(); - const bool stuck_detected = checkStuckVehicle( - planner_data_, path_lanelets, interpolated_path_info, intersection_stop_lines); + const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); - if (stuck_detected) { + if (stuck_detected && stuck_stop_line_idx_opt) { + auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); const double dist_stopline = motion_utils::calcSignedArcLength( path->points, path->points.at(closest_idx).point.pose.position, path->points.at(stuck_stop_line_idx).point.pose.position); @@ -798,24 +809,30 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool timeout = (is_private_area_ && stuck_private_area_timeout_.getState() == StateMachine::State::GO); if (!timeout) { - is_peeking_ = false; + if ( + default_stop_line_idx_opt && + motion_utils::calcSignedArcLength(path->points, stuck_stop_line_idx, closest_idx) > + planner_param_.common.stop_overshoot_margin) { + stuck_stop_line_idx = default_stop_line_idx_opt.value(); + } return IntersectionModule::StuckStop{ - stuck_stop_line_idx, !first_attention_area.has_value(), intersection_stop_lines}; + closest_idx, stuck_stop_line_idx, occlusion_peeking_stop_line_idx_opt}; } } - if (!first_attention_area) { + if (!first_attention_area_opt) { RCLCPP_DEBUG(logger_, "attention area is empty"); - is_peeking_ = false; return IntersectionModule::Indecisive{}; } + const auto first_attention_area = first_attention_area_opt.value(); - if (default_stop_line_idx == 0) { - RCLCPP_DEBUG(logger_, "stop line index is 0"); - is_peeking_ = false; + if (!default_stop_line_idx_opt) { + RCLCPP_DEBUG(logger_, "default stop line is null"); return IntersectionModule::Indecisive{}; } + const auto default_stop_line_idx = default_stop_line_idx_opt.value(); + // TODO(Mamoru Sobue): this part needs more formal handling const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; debug_data_.pass_judge_wall_pose = planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path); @@ -828,24 +845,30 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( planner_data_->current_velocity->twist.linear.y); const bool keep_detection = (vel_norm < planner_param_.collision_detection.keep_detection_vel_thr); + const bool was_safe = std::holds_alternative(prev_decision_result_); // if ego is over the pass judge line and not stopped - if (is_peeking_) { - // do nothing - RCLCPP_DEBUG(logger_, "peeking now"); - } else if (is_over_default_stop_line && !is_over_pass_judge_line && keep_detection) { + if (is_over_default_stop_line && !is_over_pass_judge_line && keep_detection) { RCLCPP_DEBUG( logger_, "is_over_default_stop_line && !is_over_pass_judge_line && keep_detection"); // do nothing } else if ( - (is_over_default_stop_line && is_over_pass_judge_line && is_go_out_) || is_permanent_go_) { + (was_safe && is_over_default_stop_line && is_over_pass_judge_line && is_go_out_) || + is_permanent_go_) { // is_go_out_: previous RTC approval // activated_: current RTC approval is_permanent_go_ = true; RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); - is_peeking_ = false; return IntersectionModule::Indecisive{}; } + if (!occlusion_peeking_stop_line_idx_opt) { + RCLCPP_DEBUG(logger_, "occlusion stop line is null"); + return IntersectionModule::Indecisive{}; + } + const auto collision_stop_line_idx = + is_over_default_stop_line ? closest_idx : default_stop_line_idx; + const auto occlusion_stop_line_idx = occlusion_peeking_stop_line_idx_opt.value(); + const auto & attention_lanelets = intersection_lanelets_.value().attention(); const auto & adjacent_lanelets = intersection_lanelets_.value().adjacent(); const auto & occlusion_attention_lanelets = intersection_lanelets_.value().occlusion_attention(); @@ -879,8 +902,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_state_machine_.getState() == StateMachine::State::STOP; if (tl_arrow_solid_on) { - is_peeking_ = false; - return TrafficLightArrowSolidOn{has_collision, intersection_stop_lines}; + return TrafficLightArrowSolidOn{ + has_collision_with_margin, closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } // check occlusion on detection lane @@ -889,6 +912,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( occlusion_attention_lanelets, routing_graph_ptr, planner_data_->occupancy_grid->info.resolution / std::sqrt(2.0)); } + const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); const double occlusion_dist_thr = std::fabs( std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) / @@ -906,15 +930,17 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !tl_arrow_solid_on) ? isOcclusionCleared( *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, - first_attention_area.value(), interpolated_path_info, - occlusion_attention_divisions_.value(), parked_attention_objects, occlusion_dist_thr) + first_attention_area, interpolated_path_info, occlusion_attention_divisions, + parked_attention_objects, occlusion_dist_thr) : true; occlusion_stop_state_machine_.setStateWithMarginTime( is_occlusion_cleared ? StateMachine::State::GO : StateMachine::STOP, logger_.get_child("occlusion_stop"), *clock_); + const bool is_occlusion_cleared_with_margin = + (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); // check safety - const bool ext_occlusion_requested = (is_occlusion_cleared && !occlusion_activated_); + const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_); if ( occlusion_stop_state_machine_.getState() == StateMachine::State::STOP || ext_occlusion_requested) { @@ -930,68 +956,45 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( before_creep_state_machine_.setState(StateMachine::State::GO); } if (before_creep_state_machine_.getState() == StateMachine::State::GO) { - if (has_collision) { - is_peeking_ = true; + if (has_collision_with_margin) { return IntersectionModule::OccludedCollisionStop{ - default_stop_line_idx, occlusion_peeking_stop_line_idx, is_occlusion_cleared, - intersection_stop_lines}; + is_occlusion_cleared_with_margin, closest_idx, collision_stop_line_idx, + occlusion_stop_line_idx}; } else { - is_peeking_ = true; return IntersectionModule::PeekingTowardOcclusion{ - occlusion_peeking_stop_line_idx, is_occlusion_cleared, intersection_stop_lines}; + is_occlusion_cleared_with_margin, closest_idx, collision_stop_line_idx, + occlusion_stop_line_idx}; } } else { if (is_stopped && approached_stop_line) { // start waiting at the first stop line before_creep_state_machine_.setState(StateMachine::State::GO); } - is_peeking_ = true; return IntersectionModule::FirstWaitBeforeOcclusion{ - default_stop_line_idx, occlusion_peeking_stop_line_idx, is_occlusion_cleared, - intersection_stop_lines}; + is_occlusion_cleared_with_margin, closest_idx, default_stop_line_idx, + occlusion_stop_line_idx}; } } else if (has_collision_with_margin) { - const bool is_over_default_stopLine = - util::isOverTargetIndex(*path, closest_idx, current_pose, default_stop_line_idx); - const auto stop_line_idx = is_over_default_stopLine ? closest_idx : default_stop_line_idx; - is_peeking_ = false; - return IntersectionModule::NonOccludedCollisionStop{stop_line_idx, intersection_stop_lines}; + return IntersectionModule::NonOccludedCollisionStop{ + closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } - is_peeking_ = false; - return IntersectionModule::Safe{intersection_stop_lines}; + return IntersectionModule::Safe{closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } bool IntersectionModule::checkStuckVehicle( - const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets, - const util::InterpolatedPathInfo & interpolated_path_info, - const util::IntersectionStopLines & intersection_stop_lines) + const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets) { const auto & objects_ptr = planner_data->predicted_objects; - const geometry_msgs::msg::Pose & current_pose = planner_data->current_odometry->pose; - const auto closest_idx = intersection_stop_lines.closest_idx; - const auto stuck_line_idx = intersection_stop_lines.stuck_stop_line; // considering lane change in the intersection, these lanelets are generated from the path - const auto & path = interpolated_path_info.path; const auto stuck_vehicle_detect_area = util::generateStuckVehicleDetectAreaPolygon( path_lanelets, planner_param_.stuck_vehicle.stuck_vehicle_detect_dist); debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); - const double dist_stuck_stopline = motion_utils::calcSignedArcLength( - path.points, path.points.at(stuck_line_idx).point.pose.position, - path.points.at(closest_idx).point.pose.position); - const bool is_over_stuck_stopline = - util::isOverTargetIndex(path, closest_idx, current_pose, stuck_line_idx) && - (dist_stuck_stopline > planner_param_.common.stop_overshoot_margin); - - bool is_stuck = false; - if (!is_over_stuck_stopline) { - is_stuck = util::checkStuckVehicleInIntersection( - objects_ptr, stuck_vehicle_detect_area, planner_param_.stuck_vehicle.stuck_vehicle_vel_thr, - &debug_data_); - } - return is_stuck; + return util::checkStuckVehicleInIntersection( + objects_ptr, stuck_vehicle_detect_area, planner_param_.stuck_vehicle.stuck_vehicle_vel_thr, + &debug_data_); } autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterTargetObjects( @@ -1522,54 +1525,54 @@ bool IntersectionModule::isOcclusionCleared( } /* -bool IntersectionModule::checkFrontVehicleDeceleration( + bool IntersectionModule::checkFrontVehicleDeceleration( lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, const Polygon2d & stuck_vehicle_detect_area, const autoware_auto_perception_msgs::msg::PredictedObject & object, const double assumed_front_car_decel) -{ + { const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; // consider vehicle in ego-lane && in front of ego const auto lon_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; const double object_decel = - planner_param_.stuck_vehicle.assumed_front_car_decel; // NOTE: this is positive + planner_param_.stuck_vehicle.assumed_front_car_decel; // NOTE: this is positive const double stopping_distance = lon_vel * lon_vel / (2 * object_decel); std::vector center_points; for (auto && p : ego_lane_with_next_lane[0].centerline()) - center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); + center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); for (auto && p : ego_lane_with_next_lane[1].centerline()) - center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); + center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); const double lat_offset = - std::fabs(motion_utils::calcLateralOffset(center_points, object_pose.position)); + std::fabs(motion_utils::calcLateralOffset(center_points, object_pose.position)); // get the nearest centerpoint to object std::vector dist_obj_center_points; for (const auto & p : center_points) - dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position, -p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_points.begin(), - std::min_element(dist_obj_center_points.begin(), dist_obj_center_points.end())); + dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position, + p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_points.begin(), + std::min_element(dist_obj_center_points.begin(), dist_obj_center_points.end())); // find two center_points whose distances from `closest_centerpoint` cross stopping_distance double acc_dist_prev = 0.0, acc_dist = 0.0; auto p1 = center_points[obj_closest_centerpoint_idx]; auto p2 = center_points[obj_closest_centerpoint_idx]; for (unsigned i = obj_closest_centerpoint_idx; i < center_points.size() - 1; ++i) { - p1 = center_points[i]; - p2 = center_points[i + 1]; - acc_dist_prev = acc_dist; - const auto arc_position_p1 = - lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p1)); - const auto arc_position_p2 = - lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p2)); - const double delta = arc_position_p2.length - arc_position_p1.length; - acc_dist += delta; - if (acc_dist > stopping_distance) { - break; - } + p1 = center_points[i]; + p2 = center_points[i + 1]; + acc_dist_prev = acc_dist; + const auto arc_position_p1 = + lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p1)); + const auto arc_position_p2 = + lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p2)); + const double delta = arc_position_p2.length - arc_position_p1.length; + acc_dist += delta; + if (acc_dist > stopping_distance) { + break; + } } // if stopping_distance >= center_points, stopping_point is center_points[end] const double ratio = (acc_dist <= stopping_distance) - ? 0.0 - : (acc_dist - stopping_distance) / (stopping_distance - acc_dist_prev); + ? 0.0 + : (acc_dist - stopping_distance) / (stopping_distance - acc_dist_prev); // linear interpolation geometry_msgs::msg::Point stopping_point; stopping_point.x = (p1.x * ratio + p2.x) / (1 + ratio); @@ -1583,18 +1586,18 @@ p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_poin autoware_auto_perception_msgs::msg::PredictedObject predicted_object = object; predicted_object.kinematics.initial_pose_with_covariance.pose.position = stopping_point; predicted_object.kinematics.initial_pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); + tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); auto predicted_obj_footprint = tier4_autoware_utils::toPolygon2d(predicted_object); const bool is_in_stuck_area = !bg::disjoint(predicted_obj_footprint, stuck_vehicle_detect_area); debug_data_.predicted_obj_pose.position = stopping_point; debug_data_.predicted_obj_pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); + tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); if (is_in_stuck_area) { - return true; + return true; } return false; -} + } */ } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 245503683e128..8063300b9b787 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -20,9 +20,9 @@ #include #include #include -#include +#include +#include #include -#include #include #include @@ -111,62 +111,55 @@ class IntersectionModule : public SceneModuleInterface } occlusion; }; - /* - enum OcclusionState { - NONE, - BEFORE_FIRST_STOP_LINE, - WAIT_FIRST_STOP_LINE, - CREEP_SECOND_STOP_LINE, - COLLISION_DETECTED, - }; - */ - using Indecisive = std::monostate; struct StuckStop { - size_t stop_line_idx; - // NOTE: this is optional because stuck vehicle detection is possible - // even if the detection area is empty. - // Still this may be required for RTC's default stop line - bool is_detection_area_empty; - util::IntersectionStopLines stop_lines; + size_t closest_idx{0}; + size_t stuck_stop_line_idx{0}; + std::optional occlusion_stop_line_idx{std::nullopt}; }; struct NonOccludedCollisionStop { - size_t stop_line_idx; - util::IntersectionStopLines stop_lines; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct FirstWaitBeforeOcclusion { - size_t first_stop_line_idx; - size_t occlusion_stop_line_idx; - bool is_actually_occlusion_cleared; - util::IntersectionStopLines stop_lines; + bool is_actually_occlusion_cleared{false}; + size_t closest_idx{0}; + size_t first_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct PeekingTowardOcclusion { - size_t stop_line_idx; // NOTE: if intersection_occlusion is disapproved externally through RTC, // it indicates "is_forcefully_occluded" - bool is_actually_occlusion_cleared; - util::IntersectionStopLines stop_lines; + bool is_actually_occlusion_cleared{false}; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct OccludedCollisionStop { - size_t stop_line_idx; - size_t occlusion_stop_line_idx; - bool is_actually_occlusion_cleared; - util::IntersectionStopLines stop_lines; + bool is_actually_occlusion_cleared{false}; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct Safe { // NOTE: if RTC is disapproved status, default stop lines are still needed. - util::IntersectionStopLines stop_lines; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct TrafficLightArrowSolidOn { - bool collision_detected; - util::IntersectionStopLines stop_lines; + bool collision_detected{false}; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; using DecisionResult = std::variant< Indecisive, // internal process error, or over the pass judge line @@ -207,14 +200,15 @@ class IntersectionModule : public SceneModuleInterface const int64_t lane_id_; const std::set associative_ids_; std::string turn_direction_; + bool is_go_out_ = false; bool is_permanent_go_ = false; - bool is_peeking_ = false; + DecisionResult prev_decision_result_; + // Parameter PlannerParam planner_param_; + std::optional intersection_lanelets_; - // for an intersection lane, its associative lanes are those that share same parent lanelet and - // have same turn_direction // for occlusion detection const bool enable_occlusion_detection_; @@ -235,7 +229,6 @@ class IntersectionModule : public SceneModuleInterface double occlusion_stop_distance_; bool occlusion_activated_ = true; // for first stop in two-phase stop - const UUID occlusion_first_stop_uuid_; bool occlusion_first_stop_required_ = false; void initializeRTCStatus(); @@ -246,9 +239,7 @@ class IntersectionModule : public SceneModuleInterface bool checkStuckVehicle( const std::shared_ptr & planner_data, - const util::PathLanelets & path_lanelets, - const util::InterpolatedPathInfo & interpolated_path_info, - const util::IntersectionStopLines & intersection_stop_lines); + const util::PathLanelets & path_lanelets); autoware_auto_perception_msgs::msg::PredictedObjects filterTargetObjects( const lanelet::ConstLanelets & attention_area_lanelets, diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index aa334f11be84d..e00b2a8b886d1 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index cd6d941b43de2..ce2d7252ac93d 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include @@ -227,15 +228,20 @@ std::optional generateIntersectionStopLines( const auto first_inside_detection_ip = first_inside_detection_idx_ip_opt.value(); // (1) default stop line position on interpolated path - int stop_idx_ip_int = 0; + bool default_stop_line_valid = true; + int stop_idx_ip_int = -1; if (const auto map_stop_idx_ip = getStopLineIndexFromMap(interpolated_path_info, planner_data, 10.0); map_stop_idx_ip) { stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; - } else { + } + if (stop_idx_ip_int < 0) { stop_idx_ip_int = static_cast(first_inside_detection_ip) - stop_line_margin_idx_dist - base2front_idx_dist; } + if (stop_idx_ip_int < 0) { + default_stop_line_valid = false; + } const auto default_stop_line_ip = stop_idx_ip_int >= 0 ? static_cast(stop_idx_ip_int) : 0; // (2) ego front stop line position on interpolated path @@ -251,13 +257,25 @@ std::optional generateIntersectionStopLines( const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); const auto area_2d = lanelet::utils::to2D(first_detection_area).basicPolygon(); int occlusion_peeking_line_ip_int = static_cast(default_stop_line_ip); - for (size_t i = default_stop_line_ip; i <= std::get<1>(lane_interval_ip); ++i) { - const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose)); - if (bg::intersects(path_footprint, area_2d)) { - occlusion_peeking_line_ip_int = i; - break; + bool occlusion_peeking_line_valid = true; + { + // NOTE: if footprints[0] is already inside the detection area, invalid + const auto & base_pose0 = path_ip.points.at(default_stop_line_ip).point.pose; + const auto path_footprint0 = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); + if (bg::intersects(path_footprint0, area_2d)) { + occlusion_peeking_line_valid = false; + } + } + if (occlusion_peeking_line_valid) { + for (size_t i = default_stop_line_ip + 1; i <= std::get<1>(lane_interval_ip); ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose)); + if (bg::intersects(path_footprint, area_2d)) { + occlusion_peeking_line_ip_int = i; + break; + } } } occlusion_peeking_line_ip_int += std::ceil(peeking_offset / ds); @@ -279,26 +297,43 @@ std::optional generateIntersectionStopLines( // (5) stuck vehicle stop line int stuck_stop_line_ip_int = 0; + bool stuck_stop_line_valid = true; if (use_stuck_stopline) { + // NOTE: when ego vehicle is approaching detection area and already passed + // first_conflicting_area, this could be null. const auto stuck_stop_line_idx_ip_opt = getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_conflicting_area); if (!stuck_stop_line_idx_ip_opt) { - return std::nullopt; + stuck_stop_line_valid = false; + stuck_stop_line_ip_int = 0; + } else { + stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value(); } - stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value(); } else { stuck_stop_line_ip_int = std::get<0>(lane_interval_ip); } - const auto stuck_stop_line_ip = static_cast( - std::max(0, stuck_stop_line_ip_int - stop_line_margin_idx_dist - base2front_idx_dist)); + stuck_stop_line_ip_int -= (stop_line_margin_idx_dist + base2front_idx_dist); + if (stuck_stop_line_ip_int < 0) { + stuck_stop_line_valid = false; + } + const auto stuck_stop_line_ip = static_cast(std::max(0, stuck_stop_line_ip_int)); - IntersectionStopLines intersection_stop_lines; + struct IntersectionStopLinesTemp + { + size_t closest_idx{0}; + size_t stuck_stop_line{0}; + size_t default_stop_line{0}; + size_t occlusion_peeking_stop_line{0}; + size_t pass_judge_line{0}; + }; + + IntersectionStopLinesTemp intersection_stop_lines_temp; std::list> stop_lines = { - {&closest_idx_ip, &intersection_stop_lines.closest_idx}, - {&stuck_stop_line_ip, &intersection_stop_lines.stuck_stop_line}, - {&default_stop_line_ip, &intersection_stop_lines.default_stop_line}, - {&occlusion_peeking_line_ip, &intersection_stop_lines.occlusion_peeking_stop_line}, - {&pass_judge_line_ip, &intersection_stop_lines.pass_judge_line}, + {&closest_idx_ip, &intersection_stop_lines_temp.closest_idx}, + {&stuck_stop_line_ip, &intersection_stop_lines_temp.stuck_stop_line}, + {&default_stop_line_ip, &intersection_stop_lines_temp.default_stop_line}, + {&occlusion_peeking_line_ip, &intersection_stop_lines_temp.occlusion_peeking_stop_line}, + {&pass_judge_line_ip, &intersection_stop_lines_temp.pass_judge_line}, }; stop_lines.sort( [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); @@ -311,14 +346,31 @@ std::optional generateIntersectionStopLines( *stop_idx = insert_idx.value(); } if ( - intersection_stop_lines.occlusion_peeking_stop_line < - intersection_stop_lines.default_stop_line) { - intersection_stop_lines.occlusion_peeking_stop_line = intersection_stop_lines.default_stop_line; + intersection_stop_lines_temp.occlusion_peeking_stop_line < + intersection_stop_lines_temp.default_stop_line) { + intersection_stop_lines_temp.occlusion_peeking_stop_line = + intersection_stop_lines_temp.default_stop_line; } if ( - intersection_stop_lines.occlusion_peeking_stop_line > intersection_stop_lines.pass_judge_line) { - intersection_stop_lines.pass_judge_line = intersection_stop_lines.occlusion_peeking_stop_line; + intersection_stop_lines_temp.occlusion_peeking_stop_line > + intersection_stop_lines_temp.pass_judge_line) { + intersection_stop_lines_temp.pass_judge_line = + intersection_stop_lines_temp.occlusion_peeking_stop_line; + } + + IntersectionStopLines intersection_stop_lines; + intersection_stop_lines.closest_idx = intersection_stop_lines_temp.closest_idx; + if (stuck_stop_line_valid) { + intersection_stop_lines.stuck_stop_line = intersection_stop_lines_temp.stuck_stop_line; + } + if (default_stop_line_valid) { + intersection_stop_lines.default_stop_line = intersection_stop_lines_temp.default_stop_line; + } + if (occlusion_peeking_line_valid) { + intersection_stop_lines.occlusion_peeking_stop_line = + intersection_stop_lines_temp.occlusion_peeking_stop_line; } + intersection_stop_lines.pass_judge_line = intersection_stop_lines_temp.pass_judge_line; return intersection_stop_lines; } @@ -327,8 +379,13 @@ std::optional getFirstPointInsidePolygon( const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, const bool search_forward) { + // NOTE: if first point is already inside the polygon, returns nullopt const auto polygon_2d = lanelet::utils::to2D(polygon); if (search_forward) { + const auto & p0 = path.points.at(lane_interval.first).point.pose.position; + if (bg::within(to_bg2d(p0), polygon_2d)) { + return std::nullopt; + } for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { const auto & p = path.points.at(i).point.pose.position; const auto is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); @@ -337,6 +394,10 @@ std::optional getFirstPointInsidePolygon( } } } else { + const auto & p0 = path.points.at(lane_interval.second).point.pose.position; + if (bg::within(to_bg2d(p0), polygon_2d)) { + return std::nullopt; + } for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { const auto & p = path.points.at(i).point.pose.position; const auto is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); @@ -704,9 +765,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()) { @@ -723,16 +785,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; @@ -1155,7 +1214,9 @@ std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, const util::InterpolatedPathInfo & interpolated_path_info, const std::set & associative_ids, const lanelet::CompoundPolygon3d & first_conflicting_area, - const std::vector & conflicting_areas, const size_t closest_idx, + const std::vector & conflicting_areas, + const std::optional & first_attention_area, + const std::vector & attention_areas, const size_t closest_idx, const double width) { const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; @@ -1195,9 +1256,13 @@ std::optional generatePathLanelets( } const auto first_inside_conflicting_idx_opt = - getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); + first_attention_area.has_value() + ? getFirstPointInsidePolygon(path, assigned_lane_interval, first_attention_area.value()) + : getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); const auto last_inside_conflicting_idx_opt = - getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); + first_attention_area.has_value() + ? getFirstPointInsidePolygons(path, assigned_lane_interval, attention_areas, false) + : getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 1361847b4b980..e766da6651417 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -112,8 +112,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, @@ -158,7 +157,9 @@ std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, const util::InterpolatedPathInfo & interpolated_path_info, const std::set & associative_ids, const lanelet::CompoundPolygon3d & first_conflicting_area, - const std::vector & conflicting_areas, const size_t closest_idx, + const std::vector & conflicting_areas, + const std::optional & first_attention_area, + const std::vector & attention_areas, const size_t closest_idx, const double width); } // namespace util diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 0af797b35b2cc..64be330a99232 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -55,10 +55,10 @@ struct DebugData struct InterpolatedPathInfo { autoware_auto_planning_msgs::msg::PathWithLaneId path; - double ds; - int lane_id; - std::set associative_lane_ids; - std::optional> lane_id_interval; + double ds{0.0}; + int lane_id{0}; + std::set associative_lane_ids{}; + std::optional> lane_id_interval{std::nullopt}; }; struct IntersectionLanelets @@ -117,19 +117,23 @@ struct IntersectionLanelets struct DiscretizedLane { - int lane_id; + int lane_id{0}; // discrete fine lines from left to right - std::vector divisions; + std::vector divisions{}; }; struct IntersectionStopLines { // NOTE: for baselink - size_t closest_idx; - size_t stuck_stop_line; - size_t default_stop_line; - size_t occlusion_peeking_stop_line; - size_t pass_judge_line; + size_t closest_idx{0}; + // NOTE: null if path does not conflict with first_conflicting_area + std::optional stuck_stop_line{std::nullopt}; + // NOTE: null if path is over map stop_line OR its value is calculated negative area + std::optional default_stop_line{std::nullopt}; + // NOTE: null if footprints do not change from outside to inside of detection area + std::optional occlusion_peeking_stop_line{std::nullopt}; + // if the value is calculated negative, its value is 0 + size_t pass_judge_line{0}; }; struct PathLanelets diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp index a4f693dfaa883..0c6bb747a854b 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp @@ -15,8 +15,9 @@ #include "scene.hpp" #include -#include -#include +#include +#include +#include #include diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp index f08154cda336c..8109b99166332 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -14,8 +14,8 @@ #include "scene.hpp" -#include "motion_utils/motion_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "util.hpp" #include diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp index 9b50a73a725db..889d0dbb064c5 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp @@ -14,7 +14,8 @@ #include "util.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include diff --git a/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp b/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp index 859ac4b4026f8..e8bf83868d122 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp @@ -16,7 +16,8 @@ #include #include -#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 77fbb18be2b5b..3e24689f2e774 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -18,7 +18,8 @@ #include #include #include -#include +#include +#include #include diff --git a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp index a8e4aee2d2815..527f6f02140ea 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index 21a8cce93a1be..029b516e93219 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -20,7 +20,9 @@ #include #include #include +#include #include +#include #include #include diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp index ef2e776112766..85ae85f1895d9 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp @@ -14,7 +14,6 @@ #include "gtest/gtest.h" #include "occlusion_spot_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "utils.hpp" #include diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp index 475e341d76528..0574a5226dc4a 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp @@ -14,6 +14,8 @@ #include "debug.hpp" +#include + #include namespace behavior_velocity_planner::out_of_lane::debug diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index 46db7a53b5c81..2ebfa4ecbe431 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -15,6 +15,10 @@ #include "decisions.hpp" #include +#include +#include +#include +#include #include #include diff --git a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp index 929fbd944af9f..9ad7437901309 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp @@ -14,7 +14,6 @@ #include "footprint.hpp" -#include #include #include diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp index 430cec2d3d4ab..280a832f6d684 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp @@ -26,6 +26,8 @@ #include #include #include +#include +#include #include #include diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp index 79c66148926fc..1a64c11a3c921 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp @@ -18,6 +18,7 @@ #include "types.hpp" #include +#include #include #include 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..c4f4edd26416c 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -16,7 +16,10 @@ #include #include +#include +#include #include +#include #include #include @@ -99,7 +102,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 +290,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..bf7c53f974ef6 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -30,6 +30,7 @@ #include #include #include +#include #include #include @@ -61,7 +62,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 +78,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/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index c33ce84ff09de..4735bb34c7b00 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -21,7 +21,9 @@ #include #include #include -#include +#include +#include +#include #include #include 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..6513a3c9a43a9 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 @@ -17,8 +17,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -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_planner_common/src/utilization/debug.cpp b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp index dc1eaeacb2f71..845b93576bec8 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include namespace behavior_velocity_planner { diff --git a/planning/behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner_common/src/utilization/util.cpp index d98e9002b2057..3f8180956cf10 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/util.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include diff --git a/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp b/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp index c4fdb83e00609..60a0ff878b478 100644 --- a/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp +++ b/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp @@ -16,6 +16,7 @@ #include #include +#include #include diff --git a/planning/behavior_velocity_run_out_module/src/debug.cpp b/planning/behavior_velocity_run_out_module/src/debug.cpp index 7fa17b0cea52a..90f69e0faae5e 100644 --- a/planning/behavior_velocity_run_out_module/src/debug.cpp +++ b/planning/behavior_velocity_run_out_module/src/debug.cpp @@ -16,7 +16,8 @@ #include "scene.hpp" -#include +#include +#include using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::calcOffsetPose; diff --git a/planning/behavior_velocity_run_out_module/src/debug.hpp b/planning/behavior_velocity_run_out_module/src/debug.hpp index 273abb10c780a..0dcd3099b616a 100644 --- a/planning/behavior_velocity_run_out_module/src/debug.hpp +++ b/planning/behavior_velocity_run_out_module/src/debug.hpp @@ -18,6 +18,9 @@ #include +#include +#include + #include #include #include diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index e2f9880435d5e..728e0fbad92a5 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -14,6 +14,11 @@ #include "dynamic_obstacle.hpp" +#include +#include +#include +#include + #include #include diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp index f9b2b12298d2d..7eece8fac29af 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp @@ -21,7 +21,6 @@ #include #include #include -#include #include diff --git a/planning/behavior_velocity_run_out_module/src/path_utils.hpp b/planning/behavior_velocity_run_out_module/src/path_utils.hpp index b01ee70f8bdfa..11ed2b7282ab3 100644 --- a/planning/behavior_velocity_run_out_module/src/path_utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/path_utils.hpp @@ -15,8 +15,6 @@ #ifndef PATH_UTILS_HPP_ #define PATH_UTILS_HPP_ -#include - #include #include #include diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index 3b2ed6a78a96c..b062bf9e3175e 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include diff --git a/planning/behavior_velocity_run_out_module/src/utils.cpp b/planning/behavior_velocity_run_out_module/src/utils.cpp index 3def2ade46731..71ab07922b27d 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.cpp +++ b/planning/behavior_velocity_run_out_module/src/utils.cpp @@ -14,6 +14,9 @@ #include "utils.hpp" +#include +#include + #include #include diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index 1f725291a38b7..74bc2489d7c32 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -17,9 +17,10 @@ #include #include -#include #include +#include + #include #include diff --git a/planning/behavior_velocity_speed_bump_module/src/debug.cpp b/planning/behavior_velocity_speed_bump_module/src/debug.cpp index 62e8cd31ec062..3f9a60d1d27e4 100644 --- a/planning/behavior_velocity_speed_bump_module/src/debug.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/debug.cpp @@ -15,8 +15,9 @@ #include "scene.hpp" #include -#include -#include +#include +#include +#include #include diff --git a/planning/behavior_velocity_speed_bump_module/src/scene.cpp b/planning/behavior_velocity_speed_bump_module/src/scene.cpp index fda92f037cf7a..dc944cc0c292b 100644 --- a/planning/behavior_velocity_speed_bump_module/src/scene.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/scene.cpp @@ -14,8 +14,8 @@ #include "scene.hpp" -#include "motion_utils/motion_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "util.hpp" #include diff --git a/planning/behavior_velocity_speed_bump_module/src/util.cpp b/planning/behavior_velocity_speed_bump_module/src/util.cpp index 89845a4b5e271..c815dab29af75 100644 --- a/planning/behavior_velocity_speed_bump_module/src/util.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/util.cpp @@ -14,10 +14,11 @@ #include "util.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include -#include +#include #include #include diff --git a/planning/behavior_velocity_stop_line_module/src/debug.cpp b/planning/behavior_velocity_stop_line_module/src/debug.cpp index 09949751f4e93..8d7de00635d4f 100644 --- a/planning/behavior_velocity_stop_line_module/src/debug.cpp +++ b/planning/behavior_velocity_stop_line_module/src/debug.cpp @@ -15,7 +15,8 @@ #include "scene.hpp" #include -#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include 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/debug.cpp b/planning/behavior_velocity_traffic_light_module/src/debug.cpp index 9e475441a3f3c..6afd5381ce315 100644 --- a/planning/behavior_velocity_traffic_light_module/src/debug.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/debug.cpp @@ -15,7 +15,8 @@ #include "scene.hpp" #include -#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include 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_virtual_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp index 1324525ebc6e1..e5cb4dcbc42e7 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp @@ -14,8 +14,9 @@ #include "scene.hpp" -#include -#include +#include +#include +#include using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::appendMarkerArray; diff --git a/planning/freespace_planner/package.xml b/planning/freespace_planner/package.xml index a748b7f80c9ad..f92c0a449970d 100644 --- a/planning/freespace_planner/package.xml +++ b/planning/freespace_planner/package.xml @@ -19,6 +19,7 @@ autoware_auto_planning_msgs freespace_planning_algorithms geometry_msgs + motion_utils nav_msgs planning_test_utils rclcpp diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp index e0d7f86e45245..025cc32d4ed24 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -32,7 +32,8 @@ #include "freespace_planning_algorithms/abstract_algorithm.hpp" -#include +#include +#include #include #include diff --git a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp index 159c0df82fcc3..2c7340072eb53 100644 --- a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -14,7 +14,8 @@ #include "freespace_planning_algorithms/abstract_algorithm.hpp" -#include +#include +#include #include diff --git a/planning/freespace_planning_algorithms/src/astar_search.cpp b/planning/freespace_planning_algorithms/src/astar_search.cpp index b2b2321adacd0..1ab5e9d5df487 100644 --- a/planning/freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/freespace_planning_algorithms/src/astar_search.cpp @@ -14,7 +14,8 @@ #include "freespace_planning_algorithms/astar_search.hpp" -#include +#include +#include #include diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index d8289824e517c..979800aea0754 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -20,7 +20,11 @@ #include #include #include -#include +#include +#include +#include +#include +#include #include #include diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp index 0bfb0553e75ef..3ea6237f38501 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -15,8 +15,6 @@ #ifndef LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ #define LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - #include #include #include diff --git a/planning/mission_planner/src/mission_planner/arrival_checker.cpp b/planning/mission_planner/src/mission_planner/arrival_checker.cpp index ac7a37acd4d5b..1e9f02ff0338d 100644 --- a/planning/mission_planner/src/mission_planner/arrival_checker.cpp +++ b/planning/mission_planner/src/mission_planner/arrival_checker.cpp @@ -14,7 +14,9 @@ #include "arrival_checker.hpp" -#include +#include +#include +#include #include diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp index 1b8a40ef8ef02..d0fc995da0ef4 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp @@ -17,7 +17,8 @@ #include "obstacle_avoidance_planner/type_alias.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp index ea2e18b099f1d..b87cfc7a1e743 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp @@ -22,7 +22,7 @@ #include "obstacle_avoidance_planner/state_equation_generator.hpp" #include "obstacle_avoidance_planner/type_alias.hpp" #include "osqp_interface/osqp_interface.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index f716f497b90da..14925a8facf88 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -15,13 +15,12 @@ #ifndef OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ #define OBSTACLE_AVOIDANCE_PLANNER__NODE_HPP_ -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/common_structs.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "obstacle_avoidance_planner/replan_checker.hpp" #include "obstacle_avoidance_planner/type_alias.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include diff --git a/planning/obstacle_avoidance_planner/src/debug_marker.cpp b/planning/obstacle_avoidance_planner/src/debug_marker.cpp index 79f3dfd2fe2e4..8bf46c697104a 100644 --- a/planning/obstacle_avoidance_planner/src/debug_marker.cpp +++ b/planning/obstacle_avoidance_planner/src/debug_marker.cpp @@ -14,7 +14,6 @@ #include "obstacle_avoidance_planner/debug_marker.hpp" -#include "motion_utils/motion_utils.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "visualization_msgs/msg/marker_array.hpp" diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 0a55cc8d91f8e..359485fadcfff 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -15,11 +15,12 @@ #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/utils/geometry_utils.hpp" #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" #include "tf2/utils.h" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" #include #include diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 6c4730e9d86f9..b5dd0e91a541e 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -15,6 +15,7 @@ #include "obstacle_avoidance_planner/node.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" +#include "motion_utils/marker/marker_helper.hpp" #include "obstacle_avoidance_planner/debug_marker.hpp" #include "obstacle_avoidance_planner/utils/geometry_utils.hpp" #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" diff --git a/planning/obstacle_avoidance_planner/src/replan_checker.cpp b/planning/obstacle_avoidance_planner/src/replan_checker.cpp index 8d281ced9a5ac..6d7c6e702471a 100644 --- a/planning/obstacle_avoidance_planner/src/replan_checker.cpp +++ b/planning/obstacle_avoidance_planner/src/replan_checker.cpp @@ -14,9 +14,10 @@ #include "obstacle_avoidance_planner/replan_checker.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" #include diff --git a/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp index 67b500571fa06..3f35de9147e6a 100644 --- a/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/geometry_utils.cpp @@ -14,15 +14,21 @@ #include "obstacle_avoidance_planner/utils/geometry_utils.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "tf2/utils.h" +#include + #include "autoware_auto_planning_msgs/msg/path_point.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/pose.hpp" +#include +#include +#include + #include #include #include diff --git a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp index 8390cf379586b..e86782c9cb0cf 100644 --- a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp @@ -14,7 +14,9 @@ #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/resample/resample.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/mpt_optimizer.hpp" #include "obstacle_avoidance_planner/utils/geometry_utils.hpp" #include "tf2/utils.h" diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 08ebea4284bf1..f3f1932c44c43 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -20,7 +20,11 @@ nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index min_behavior_stop_margin: 3.0 # [m] - suppress_sudden_obstacle_stop: false + stop_on_curve: + enable_approaching: true # false + additional_safe_distance_margin: 0.0 # [m] + min_safe_distance_margin: 3.0 # [m] + suppress_sudden_obstacle_stop: true stop_obstacle_type: unknown: true @@ -54,7 +58,7 @@ pedestrian: false slow_down_obstacle_type: - unknown: true + unknown: false car: true truck: true bus: true diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index 12ebadf770996..e958074971f2a 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -15,9 +15,13 @@ #ifndef OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ #define OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/interpolation.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_cruise_planner/type_alias.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/ros/uuid_helper.hpp" #include @@ -104,12 +108,15 @@ struct StopObstacle : public TargetObstacleInterface { StopObstacle( const std::string & arg_uuid, const rclcpp::Time & arg_stamp, - const geometry_msgs::msg::Pose & arg_pose, const double arg_lon_velocity, - const double arg_lat_velocity, const geometry_msgs::msg::Point arg_collision_point) + const geometry_msgs::msg::Pose & arg_pose, const Shape & arg_shape, + const double arg_lon_velocity, const double arg_lat_velocity, + const geometry_msgs::msg::Point arg_collision_point) : TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity), + shape(arg_shape), collision_point(arg_collision_point) { } + Shape shape; geometry_msgs::msg::Point collision_point; }; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index ac6684d163aea..00afc11985d72 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -100,6 +100,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool enable_debug_info_; bool enable_calculation_time_info_; double min_behavior_stop_margin_; + bool enable_approaching_on_curve_; + double additional_safe_distance_margin_on_curve_; + double min_safe_distance_margin_on_curve_; bool suppress_sudden_obstacle_stop_; std::vector stop_obstacle_types_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp index 55a97ccc2785d..44626206ec038 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp @@ -19,7 +19,6 @@ #include "obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp" #include "obstacle_cruise_planner/planner_interface.hpp" #include "obstacle_cruise_planner/type_alias.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index c3fa364da269e..9a5a6521a6494 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -15,12 +15,13 @@ #ifndef OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ #define OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_cruise_planner/common_structs.hpp" #include "obstacle_cruise_planner/stop_planning_debug_info.hpp" #include "obstacle_cruise_planner/type_alias.hpp" #include "obstacle_cruise_planner/utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include @@ -53,11 +54,16 @@ class PlannerInterface void setParam( const bool enable_debug_info, const bool enable_calculation_time_info, - const double min_behavior_stop_margin, const bool suppress_sudden_obstacle_stop) + const double min_behavior_stop_margin, const double enable_approaching_on_curve, + const double additional_safe_distance_margin_on_curve, + const double min_safe_distance_margin_on_curve, const bool suppress_sudden_obstacle_stop) { enable_debug_info_ = enable_debug_info; enable_calculation_time_info_ = enable_calculation_time_info; min_behavior_stop_margin_ = min_behavior_stop_margin; + enable_approaching_on_curve_ = enable_approaching_on_curve; + additional_safe_distance_margin_on_curve_ = additional_safe_distance_margin_on_curve; + min_safe_distance_margin_on_curve_ = min_safe_distance_margin_on_curve; suppress_sudden_obstacle_stop_ = suppress_sudden_obstacle_stop; } @@ -102,6 +108,9 @@ class PlannerInterface bool enable_calculation_time_info_{false}; LongitudinalInfo longitudinal_info_; double min_behavior_stop_margin_; + bool enable_approaching_on_curve_; + double additional_safe_distance_margin_on_curve_; + double min_safe_distance_margin_on_curve_; bool suppress_sudden_obstacle_stop_; // stop watch @@ -194,6 +203,8 @@ class PlannerInterface std::optional start_point{std::nullopt}; std::optional end_point{std::nullopt}; }; + double calculateMarginFromObstacleOnCurve( + const PlannerData & planner_data, const StopObstacle & stop_obstacle) const; double calculateSlowDownVelocity( const SlowDownObstacle & obstacle, const std::optional & prev_output) const; std::optional> calculateDistanceToSlowDownWithConstraints( diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp index fdbf5b413d978..5178597bc4588 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp @@ -17,7 +17,7 @@ #include "obstacle_cruise_planner/common_structs.hpp" #include "obstacle_cruise_planner/type_alias.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp index ed6d0f52691e3..c635b400ed104 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp @@ -16,9 +16,8 @@ #define OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ #include "common_structs.hpp" -#include "motion_utils/motion_utils.hpp" #include "obstacle_cruise_planner/type_alias.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 266e184a06a08..14324709ba9e1 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -20,6 +20,7 @@ #include "obstacle_cruise_planner/polygon_utils.hpp" #include "obstacle_cruise_planner/utils.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include @@ -397,11 +398,18 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & } min_behavior_stop_margin_ = declare_parameter("common.min_behavior_stop_margin"); + additional_safe_distance_margin_on_curve_ = + declare_parameter("common.stop_on_curve.additional_safe_distance_margin"); + enable_approaching_on_curve_ = + declare_parameter("common.stop_on_curve.enable_approaching"); + min_safe_distance_margin_on_curve_ = + declare_parameter("common.stop_on_curve.min_safe_distance_margin"); suppress_sudden_obstacle_stop_ = declare_parameter("common.suppress_sudden_obstacle_stop"); planner_ptr_->setParam( enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_, - suppress_sudden_obstacle_stop_); + enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, + min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); } { // stop/cruise/slow down obstacle type @@ -438,9 +446,20 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( parameters, "common.enable_debug_info", enable_debug_info_); tier4_autoware_utils::updateParam( parameters, "common.enable_calculation_time_info", enable_calculation_time_info_); + + tier4_autoware_utils::updateParam( + parameters, "common.stop_on_curve.enable_approaching", enable_approaching_on_curve_); + tier4_autoware_utils::updateParam( + parameters, "common.stop_on_curve.additional_safe_distance_margin", + additional_safe_distance_margin_on_curve_); + tier4_autoware_utils::updateParam( + parameters, "common.stop_on_curve.min_safe_distance_margin", + min_safe_distance_margin_on_curve_); + planner_ptr_->setParam( enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_, - suppress_sudden_obstacle_stop_); + enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, + min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); tier4_autoware_utils::updateParam( parameters, "common.enable_slow_down_planning", enable_slow_down_planning_); @@ -911,7 +930,7 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( } const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); - return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose, + return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose, obstacle.shape, tangent_vel, normal_vel, *collision_point}; } diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index f420993c1adac..08f893a0a9a50 100644 --- a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -17,11 +17,13 @@ #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/zero_order_hold.hpp" +#include "motion_utils/marker/marker_helper.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_cruise_planner/utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" #ifdef ROS_DISTRO_GALACTIC #include diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index ffd2f97b1eb89..005a85d9b872d 100644 --- a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -15,8 +15,11 @@ #include "obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" #include "interpolation/spline_interpolation.hpp" +#include "motion_utils/marker/marker_helper.hpp" #include "obstacle_cruise_planner/utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" #include "tier4_planning_msgs/msg/velocity_limit.hpp" diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 03cae6e6f9d88..5ea637655a01a 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -15,7 +15,12 @@ #include "obstacle_cruise_planner/planner_interface.hpp" #include "motion_utils/distance/distance.hpp" +#include "motion_utils/marker/marker_helper.hpp" +#include "motion_utils/resample/resample.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "signal_processing/lowpass_filter_1d.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" namespace { @@ -204,6 +209,19 @@ double calcDecelerationVelocityFromDistanceToTarget( } return current_velocity; } + +std::vector resampleTrajectoryPoints( + const std::vector & traj_points, const double interval) +{ + const auto traj = motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = motion_utils::resampleTrajectory(traj, interval); + return motion_utils::convertToTrajectoryPointArray(resampled_traj); +} + +tier4_autoware_utils::Point2d convertPoint(const geometry_msgs::msg::Point & p) +{ + return tier4_autoware_utils::Point2d{p.x, p.y}; +} } // namespace std::vector PlannerInterface::generateStopTrajectory( @@ -259,16 +277,19 @@ std::vector PlannerInterface::generateStopTrajectory( planner_data.traj_points, planner_data.ego_pose.position, ego_segment_idx, 0); const double dist_to_ego = -negative_dist_to_ego; + const double margin_from_obstacle = + calculateMarginFromObstacleOnCurve(planner_data, *closest_stop_obstacle); + // If behavior stop point is ahead of the closest_obstacle_stop point within a certain margin // we set closest_obstacle_stop_distance to closest_behavior_stop_distance - const double margin_from_obstacle = [&]() { + const double margin_from_obstacle_considering_behavior_module = [&]() { const size_t nearest_segment_idx = findEgoSegmentIndex(planner_data.traj_points, planner_data.ego_pose); const auto closest_behavior_stop_idx = motion_utils::searchZeroVelocityIndex(planner_data.traj_points, nearest_segment_idx + 1); if (!closest_behavior_stop_idx) { - return longitudinal_info_.safe_distance_margin; + return margin_from_obstacle; } const double closest_behavior_stop_dist_from_ego = motion_utils::calcSignedArcLength( @@ -282,29 +303,28 @@ std::vector PlannerInterface::generateStopTrajectory( abs_ego_offset; const double stop_dist_diff = closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego; - if (stop_dist_diff < longitudinal_info_.safe_distance_margin) { + if (stop_dist_diff < margin_from_obstacle) { // Use terminal margin (terminal_safe_distance_margin) for obstacle stop return longitudinal_info_.terminal_safe_distance_margin; } } else { - const double closest_obstacle_stop_dist_from_ego = closest_obstacle_dist - dist_to_ego - - longitudinal_info_.safe_distance_margin - - abs_ego_offset; + const double closest_obstacle_stop_dist_from_ego = + closest_obstacle_dist - dist_to_ego - margin_from_obstacle - abs_ego_offset; const double stop_dist_diff = closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego; - if (0.0 < stop_dist_diff && stop_dist_diff < longitudinal_info_.safe_distance_margin) { + if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) { // Use shorter margin (min_behavior_stop_margin) for obstacle stop return min_behavior_stop_margin_; } } - return longitudinal_info_.safe_distance_margin; + return margin_from_obstacle; }(); const auto [stop_margin_from_obstacle, will_collide_with_obstacle] = [&]() { // Check feasibility to stop if (suppress_sudden_obstacle_stop_) { const double closest_obstacle_stop_dist = - closest_obstacle_dist - margin_from_obstacle - abs_ego_offset; + closest_obstacle_dist - margin_from_obstacle_considering_behavior_module - abs_ego_offset; // Calculate feasible stop margin (Check the feasibility) const double feasible_stop_dist = calcMinimumDistanceToStop( @@ -314,11 +334,12 @@ std::vector PlannerInterface::generateStopTrajectory( if (closest_obstacle_stop_dist < feasible_stop_dist) { const auto feasible_margin_from_obstacle = - margin_from_obstacle - (feasible_stop_dist - closest_obstacle_stop_dist); + margin_from_obstacle_considering_behavior_module - + (feasible_stop_dist - closest_obstacle_stop_dist); return std::make_pair(feasible_margin_from_obstacle, true); } } - return std::make_pair(margin_from_obstacle, false); + return std::make_pair(margin_from_obstacle_considering_behavior_module, false); }(); // Generate Output Trajectory @@ -395,6 +416,91 @@ std::vector PlannerInterface::generateStopTrajectory( return output_traj_points; } +double PlannerInterface::calculateMarginFromObstacleOnCurve( + const PlannerData & planner_data, const StopObstacle & stop_obstacle) const +{ + if (!enable_approaching_on_curve_) { + return longitudinal_info_.safe_distance_margin; + } + + const double abs_ego_offset = planner_data.is_driving_forward + ? std::abs(vehicle_info_.max_longitudinal_offset_m) + : std::abs(vehicle_info_.min_longitudinal_offset_m); + + // calculate short trajectory points towards obstacle + const size_t obj_segment_idx = + motion_utils::findNearestSegmentIndex(planner_data.traj_points, stop_obstacle.collision_point); + std::vector short_traj_points{planner_data.traj_points.at(obj_segment_idx + 1)}; + double sum_short_traj_length{0.0}; + for (int i = obj_segment_idx; 0 <= i; --i) { + short_traj_points.push_back(planner_data.traj_points.at(i)); + + if ( + 1 < short_traj_points.size() && + longitudinal_info_.safe_distance_margin + abs_ego_offset < sum_short_traj_length) { + break; + } + sum_short_traj_length += tier4_autoware_utils::calcDistance2d( + planner_data.traj_points.at(i), planner_data.traj_points.at(i + 1)); + } + std::reverse(short_traj_points.begin(), short_traj_points.end()); + if (short_traj_points.size() < 2) { + return longitudinal_info_.safe_distance_margin; + } + + // calculate collision index between straight line from ego pose and object + const auto calculate_distance_from_straight_ego_path = + [&](const auto & ego_pose, const auto & object_polygon) { + const auto forward_ego_pose = tier4_autoware_utils::calcOffsetPose( + ego_pose, longitudinal_info_.safe_distance_margin + 3.0, 0.0, 0.0); + const auto ego_straight_segment = tier4_autoware_utils::Segment2d{ + convertPoint(ego_pose.position), convertPoint(forward_ego_pose.position)}; + return boost::geometry::distance(ego_straight_segment, object_polygon); + }; + const auto resampled_short_traj_points = resampleTrajectoryPoints(short_traj_points, 0.5); + const auto object_polygon = + tier4_autoware_utils::toPolygon2d(stop_obstacle.pose, stop_obstacle.shape); + const auto collision_idx = [&]() -> std::optional { + for (size_t i = 0; i < resampled_short_traj_points.size(); ++i) { + const double dist_to_obj = calculate_distance_from_straight_ego_path( + resampled_short_traj_points.at(i).pose, object_polygon); + if (dist_to_obj < vehicle_info_.vehicle_width_m / 2.0) { + return i; + } + } + return std::nullopt; + }(); + if (!collision_idx) { + return min_safe_distance_margin_on_curve_; + } + if (*collision_idx == 0) { + return longitudinal_info_.safe_distance_margin; + } + + // calculate margin from obstacle + const double partial_segment_length = [&]() { + const double collision_segment_length = tier4_autoware_utils::calcDistance2d( + resampled_short_traj_points.at(*collision_idx - 1), + resampled_short_traj_points.at(*collision_idx)); + const double prev_dist = calculate_distance_from_straight_ego_path( + resampled_short_traj_points.at(*collision_idx - 1).pose, object_polygon); + const double next_dist = calculate_distance_from_straight_ego_path( + resampled_short_traj_points.at(*collision_idx).pose, object_polygon); + return (next_dist - vehicle_info_.vehicle_width_m / 2.0) / (next_dist - prev_dist) * + collision_segment_length; + }(); + + const double short_margin_from_obstacle = + partial_segment_length + + motion_utils::calcSignedArcLength( + resampled_short_traj_points, *collision_idx, stop_obstacle.collision_point) - + abs_ego_offset + additional_safe_distance_margin_on_curve_; + + return std::min( + longitudinal_info_.safe_distance_margin, + std::max(min_safe_distance_margin_on_curve_, short_margin_from_obstacle)); +} + double PlannerInterface::calcDistanceToCollisionPoint( const PlannerData & planner_data, const geometry_msgs::msg::Point & collision_point) { diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp index 091adfab586b4..42bac55e63dd8 100644 --- a/planning/obstacle_cruise_planner/src/utils.cpp +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -15,6 +15,7 @@ #include "obstacle_cruise_planner/utils.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" namespace obstacle_cruise_utils { diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp index 40a3f36bff903..9d95c5a4796d4 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -16,7 +16,6 @@ #define OBSTACLE_STOP_PLANNER__ADAPTIVE_CRUISE_CONTROL_HPP_ #include -#include #include #include diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index c836b380e5cf2..4cb71c710a225 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -23,8 +23,7 @@ #include #include #include -#include -#include +#include #include #include diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp index 8023e86583885..7e7e659325abf 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp @@ -18,7 +18,7 @@ #include "obstacle_stop_planner/planner_data.hpp" -#include +#include #include #include @@ -46,7 +46,6 @@ using diagnostic_msgs::msg::KeyValue; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; -using geometry_msgs::msg::TransformStamped; using std_msgs::msg::Header; using autoware_auto_planning_msgs::msg::TrajectoryPoint; diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index fb1e93185f738..8064aa4764a6d 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -28,6 +28,8 @@ #include #endif +#include + #include #include #include diff --git a/planning/obstacle_stop_planner/src/debug_marker.cpp b/planning/obstacle_stop_planner/src/debug_marker.cpp index 15b0a721ed170..9277d373d498d 100644 --- a/planning/obstacle_stop_planner/src/debug_marker.cpp +++ b/planning/obstacle_stop_planner/src/debug_marker.cpp @@ -14,8 +14,9 @@ #include "obstacle_stop_planner/debug_marker.hpp" -#include -#include +#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include diff --git a/planning/obstacle_stop_planner/src/planner_utils.cpp b/planning/obstacle_stop_planner/src/planner_utils.cpp index 45cced4599602..21e45cac79fdf 100644 --- a/planning/obstacle_stop_planner/src/planner_utils.cpp +++ b/planning/obstacle_stop_planner/src/planner_utils.cpp @@ -17,10 +17,13 @@ #include #include #include +#include #include #include +#include +#include #include diff --git a/planning/path_smoother/include/path_smoother/common_structs.hpp b/planning/path_smoother/include/path_smoother/common_structs.hpp index 94f4d62c4a6fd..a14c42ed056af 100644 --- a/planning/path_smoother/include/path_smoother/common_structs.hpp +++ b/planning/path_smoother/include/path_smoother/common_structs.hpp @@ -17,7 +17,8 @@ #include "path_smoother/type_alias.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include diff --git a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp index 1085ca3815728..8bd73292a56de 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp @@ -15,13 +15,12 @@ #ifndef PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_ #define PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_ -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "path_smoother/common_structs.hpp" #include "path_smoother/elastic_band.hpp" #include "path_smoother/replan_checker.hpp" #include "path_smoother/type_alias.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include #include diff --git a/planning/path_smoother/src/elastic_band.cpp b/planning/path_smoother/src/elastic_band.cpp index d77ad542e2e7e..fb3aff7b18419 100644 --- a/planning/path_smoother/src/elastic_band.cpp +++ b/planning/path_smoother/src/elastic_band.cpp @@ -14,7 +14,7 @@ #include "path_smoother/elastic_band.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "path_smoother/type_alias.hpp" #include "path_smoother/utils/geometry_utils.hpp" #include "path_smoother/utils/trajectory_utils.hpp" diff --git a/planning/path_smoother/src/utils/trajectory_utils.cpp b/planning/path_smoother/src/utils/trajectory_utils.cpp index 56432d16d1a7f..d9d02a15628dd 100644 --- a/planning/path_smoother/src/utils/trajectory_utils.cpp +++ b/planning/path_smoother/src/utils/trajectory_utils.cpp @@ -14,7 +14,8 @@ #include "path_smoother/utils/trajectory_utils.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/resample/resample.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" #include "path_smoother/utils/geometry_utils.hpp" #include "autoware_auto_planning_msgs/msg/path_point.hpp" diff --git a/planning/planning_debug_tools/Readme.md b/planning/planning_debug_tools/README.md similarity index 100% rename from planning/planning_debug_tools/Readme.md rename to planning/planning_debug_tools/README.md 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..79a97f0033b18 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py @@ -20,6 +20,7 @@ import sys from PyQt5.QtWidgets import QApplication +from geometry_msgs.msg import PoseWithCovarianceStamped from perception_replayer_common import PerceptionReplayerCommon import rclpy from time_manager_widget import TimeManagerWidget @@ -34,6 +35,7 @@ def __init__(self, args): self.bag_timestamp = self.rosbag_objects_data[0][0] self.is_pause = False self.rate = 1.0 + self.prev_traffic_signals_msg = None # initialize widget self.widget = TimeManagerWidget( @@ -43,6 +45,7 @@ def __init__(self, args): self.widget.button.clicked.connect(self.onPushed) for button in self.widget.rate_button: button.clicked.connect(functools.partial(self.onSetRate, button)) + self.widget.pub_recorded_ego_pose_button.clicked.connect(self.publish_recorded_ego_pose) # start timer callback self.delta_time = 0.1 @@ -92,13 +95,22 @@ def on_timer(self): self.objects_pub.publish(objects_msg) # traffic signals + # temporary support old auto msgs if traffic_signals_msg: - traffic_signals_msg.header.stamp = timestamp - self.traffic_signals_pub.publish(traffic_signals_msg) + if self.is_auto_traffic_signals: + traffic_signals_msg.header.stamp = timestamp + self.auto_traffic_signals_pub.publish(traffic_signals_msg) + else: + 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.traffic_signals_pub.publish(self.prev_traffic_signals_msg) + if self.is_auto_traffic_signals: + self.prev_traffic_signals_msg.header.stamp = timestamp + self.auto_traffic_signals_pub.publish(self.prev_traffic_signals_msg) + else: + self.prev_traffic_signals_msg.stamp = timestamp + self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) def onPushed(self, event): if self.widget.button.isChecked(): @@ -109,6 +121,57 @@ def onPushed(self, event): def onSetRate(self, button): self.rate = float(button.text()) + def publish_recorded_ego_pose(self): + ego_odom = self.find_ego_odom_by_timestamp(self.bag_timestamp) + if not ego_odom: + return + + recorded_ego_pose = PoseWithCovarianceStamped() + recorded_ego_pose.header.stamp = self.get_clock().now().to_msg() + recorded_ego_pose.header.frame_id = "map" + recorded_ego_pose.pose.pose = ego_odom.pose.pose + recorded_ego_pose.pose.covariance = [ + 0.25, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.25, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.06853892326654787, + ] + + self.recorded_ego_pub.publish(recorded_ego_pose) + print("Published recorded ego pose as /initialpose") + if __name__ == "__main__": parser = argparse.ArgumentParser() 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..6b758bfba24ee 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,9 @@ 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_auto_perception_msgs.msg import TrafficSignalArray as AutoTrafficSignalArray +from autoware_perception_msgs.msg import TrafficSignalArray +from geometry_msgs.msg import PoseWithCovarianceStamped from nav_msgs.msg import Odometry import psutil from rclpy.node import Node @@ -42,6 +44,7 @@ def __init__(self, args, name): self.rosbag_objects_data = [] self.rosbag_ego_odom_data = [] self.rosbag_traffic_signals_data = [] + self.is_auto_traffic_signals = False # subscriber self.sub_odom = self.create_subscription( @@ -65,9 +68,7 @@ def __init__(self, args, name): self.pointcloud_pub = self.create_publisher( PointCloud2, "/perception/obstacle_segmentation/pointcloud", 1 ) - self.traffic_signals_pub = self.create_publisher( - TrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 - ) + self.recorded_ego_pub = self.create_publisher(PoseWithCovarianceStamped, "/initialpose", 1) # load rosbag print("Stared loading rosbag") @@ -78,6 +79,16 @@ def __init__(self, args, name): self.load_rosbag(args.bag) print("Ended loading rosbag") + # temporary support old auto msgs + if self.is_auto_traffic_signals: + self.auto_traffic_signals_pub = self.create_publisher( + AutoTrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 + ) + else: + self.traffic_signals_pub = self.create_publisher( + TrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 + ) + # wait for ready to publish/subscribe time.sleep(1.0) @@ -113,6 +124,9 @@ def load_rosbag(self, rosbag2_path: str): self.rosbag_ego_odom_data.append((stamp, msg)) if topic == traffic_signals_topic: self.rosbag_traffic_signals_data.append((stamp, msg)) + self.is_auto_traffic_signals = ( + "autoware_auto_perception_msgs" in type(msg).__module__ + ) def kill_online_perception_node(self): # kill node if required 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..1b75c86de2709 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py @@ -66,13 +66,22 @@ def on_timer(self): self.objects_pub.publish(objects_msg) # traffic signals + # temporary support old auto msgs if traffic_signals_msg: - traffic_signals_msg.header.stamp = timestamp - self.traffic_signals_pub.publish(traffic_signals_msg) + if self.is_auto_traffic_signals: + traffic_signals_msg.header.stamp = timestamp + self.auto_traffic_signals_pub.publish(traffic_signals_msg) + else: + 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.traffic_signals_pub.publish(self.prev_traffic_signals_msg) + if self.is_auto_traffic_signals: + self.prev_traffic_signals_msg.header.stamp = timestamp + self.auto_traffic_signals_pub.publish(self.prev_traffic_signals_msg) + else: + 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): if self.ego_pose_idx: diff --git a/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py b/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py index 382e725b114cd..a1d87a8b06266 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/time_manager_widget.py @@ -89,13 +89,15 @@ def setupUI(self): self.button.setCheckable(True) self.button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) self.grid_layout.addWidget(self.button, 1, 0, 1, -1) + self.pub_recorded_ego_pose_button = QPushButton("publish recorded ego pose") + self.grid_layout.addWidget(self.pub_recorded_ego_pose_button, 2, 0, 1, -1) # slider self.slider = QJumpSlider(QtCore.Qt.Horizontal, self.max_value) self.slider.setMinimum(0) self.slider.setMaximum(self.max_value) self.slider.setValue(0) - self.grid_layout.addWidget(self.slider, 2, 0, 1, -1) + self.grid_layout.addWidget(self.slider, 3, 0, 1, -1) self.setCentralWidget(self.central_widget) diff --git a/planning/planning_debug_tools/scripts/trajectory_visualizer.py b/planning/planning_debug_tools/scripts/trajectory_visualizer.py index f942d9a00c098..73e41b052cd90 100755 --- a/planning/planning_debug_tools/scripts/trajectory_visualizer.py +++ b/planning/planning_debug_tools/scripts/trajectory_visualizer.py @@ -17,8 +17,11 @@ import argparse from autoware_auto_planning_msgs.msg import Path +from autoware_auto_planning_msgs.msg import PathPoint +from autoware_auto_planning_msgs.msg import PathPointWithLaneId from autoware_auto_planning_msgs.msg import PathWithLaneId from autoware_auto_planning_msgs.msg import Trajectory +from autoware_auto_planning_msgs.msg import TrajectoryPoint from autoware_auto_vehicle_msgs.msg import VelocityReport from geometry_msgs.msg import Pose from matplotlib import animation @@ -201,7 +204,7 @@ def CallbackVelocityLimit(self, cmd): self.velocity_limit = cmd.max_velocity def CallbackMotionVelOptTraj(self, cmd1, cmd2, cmd3, cmd4, cmd5): - print("CallbackMotionVelOptTraj called") + self.get_logger().info("CallbackMotionVelOptTraj called") self.CallBackTrajExVelLim(cmd1) self.CallBackTrajLatAccFiltered(cmd2) self.CallBackTrajRaw(cmd3) @@ -233,7 +236,7 @@ def CallBackTrajFinal(self, cmd): self.update_traj_final = True def CallBackLaneDrivingTraj(self, cmd5, cmd6, cmd7, cmd8, cmd9): - print("CallBackLaneDrivingTraj called") + self.get_logger().info("CallBackLaneDrivingTraj called") self.CallBackTrajFinal(cmd5) self.CallBackLBehaviorPathPlannerPath(cmd6) self.CallBackBehaviorVelocityPlannerPath(cmd7) @@ -306,9 +309,8 @@ def setPlotTrajectoryVelocity(self): ) def plotTrajectoryVelocity(self, data): - # self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME) if self.self_pose_received is False: - print("plot start but self pose is not received") + self.get_logger().info("plot start but self pose is not received") return ( self.im1, self.im2, @@ -324,82 +326,9 @@ def plotTrajectoryVelocity(self, data): self.im11, self.im12, ) - print("plot start") - - # copy - behavior_path_planner_path = self.behavior_path_planner_path - behavior_velocity_planner_path = self.behavior_velocity_planner_path - obstacle_avoid_traj = self.obstacle_avoid_traj - obstacle_stop_traj = self.obstacle_stop_traj - trajectory_raw = self.trajectory_raw - trajectory_external_velocity_limited = self.trajectory_external_velocity_limited - trajectory_lateral_acc_filtered = self.trajectory_lateral_acc_filtered - trajectory_steer_rate_filtered = self.trajectory_steer_rate_filtered - trajectory_time_resampled = self.trajectory_time_resampled - trajectory_final = self.trajectory_final - - if self.update_behavior_path_planner_path: - x = self.CalcArcLengthPathWLid(behavior_path_planner_path) - y = self.ToVelListPathWLid(behavior_path_planner_path) - self.im1.set_data(x, y) - self.update_behavior_path_planner_path = False - if len(y) != 0: - self.max_vel = max(10.0, np.max(y)) - - if self.update_behavior_velocity_planner_path: - x = self.CalcArcLengthPath(behavior_velocity_planner_path) - y = self.ToVelListPath(behavior_velocity_planner_path) - self.im2.set_data(x, y) - self.update_behavior_velocity_planner_path = False - - if self.update_traj_ob_avoid: - x = self.CalcArcLength(obstacle_avoid_traj) - y = self.ToVelList(obstacle_avoid_traj) - self.im3.set_data(x, y) - self.update_traj_ob_avoid = False - - if self.update_traj_ob_stop: - x = self.CalcArcLength(obstacle_stop_traj) - y = self.ToVelList(obstacle_stop_traj) - self.im4.set_data(x, y) - self.update_traj_ob_stop = False - - if self.update_traj_raw: - x = self.CalcArcLength(trajectory_raw) - y = self.ToVelList(trajectory_raw) - self.im5.set_data(x, y) - self.update_traj_raw = False - - if self.update_ex_vel_lim: - x = self.CalcArcLength(trajectory_external_velocity_limited) - y = self.ToVelList(trajectory_external_velocity_limited) - self.im6.set_data(x, y) - self.update_ex_vel_lim = False - - if self.update_lat_acc_fil: - x = self.CalcArcLength(trajectory_lateral_acc_filtered) - y = self.ToVelList(trajectory_lateral_acc_filtered) - self.im7.set_data(x, y) - self.update_lat_acc_fil = False - - if self.update_steer_rate_fil: - x = self.CalcArcLength(trajectory_steer_rate_filtered) - y = self.ToVelList(trajectory_steer_rate_filtered) - self.im71.set_data(x, y) - self.update_steer_rate_fil = False - - if self.update_traj_resample: - x = self.CalcArcLength(trajectory_time_resampled) - y = self.ToVelList(trajectory_time_resampled) - self.im8.set_data(x, y) - self.update_traj_resample = False + self.get_logger().info("plot start") if self.update_traj_final: - x = self.CalcArcLength(trajectory_final) - y = self.ToVelList(trajectory_final) - self.im9.set_data(x, y) - self.update_traj_final = False - self.im10.set_data(0, self.localization_vx) self.im11.set_data(0, self.vehicle_vx) @@ -408,8 +337,33 @@ def plotTrajectoryVelocity(self, data): y = [self.velocity_limit, self.velocity_limit] self.im12.set_data(x, y) - if len(y) != 0: - self.min_vel = np.min(y) + if len(y) != 0: + self.min_vel = np.min(y) + + trajectory_data = [ + (self.behavior_path_planner_path, self.update_behavior_path_planner_path, self.im1), + ( + self.behavior_velocity_planner_path, + self.update_behavior_velocity_planner_path, + self.im2, + ), + (self.obstacle_avoid_traj, self.update_traj_ob_avoid, self.im3), + (self.obstacle_stop_traj, self.update_traj_ob_stop, self.im4), + (self.trajectory_raw, self.update_traj_raw, self.im5), + (self.trajectory_external_velocity_limited, self.update_ex_vel_lim, self.im6), + (self.trajectory_lateral_acc_filtered, self.update_lat_acc_fil, self.im7), + (self.trajectory_steer_rate_filtered, self.update_steer_rate_fil, self.im71), + (self.trajectory_time_resampled, self.update_traj_resample, self.im8), + (self.trajectory_final, self.update_traj_final, self.im9), + ] + + # update all trajectory plots + for traj, update_flag, image in trajectory_data: + if update_flag: + x = self.CalcArcLength(traj) + y = self.ToVelList(traj) + image.set_data(x, y) + update_flag = False return ( self.im1, @@ -427,107 +381,43 @@ def plotTrajectoryVelocity(self, data): self.im12, ) - def CalcArcLength(self, traj): - if len(traj.points) == 0: - return - - s_arr = [] - ds = 0.0 - s_sum = 0.0 - - closest_id = self.calcClosestTrajectory(traj) - for i in range(1, closest_id): - p0 = traj.points[i - 1] - p1 = traj.points[i] - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum -= ds - - s_arr.append(s_sum) - for i in range(1, len(traj.points)): - p0 = traj.points[i - 1] - p1 = traj.points[i] - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum += ds - s_arr.append(s_sum) - return s_arr - - def CalcArcLengthPathWLid(self, traj): - if len(traj.points) == 0: - return + def getPoint(self, path_point): + if isinstance(path_point, (TrajectoryPoint, PathPoint)): + return path_point + elif isinstance(path_point, PathPointWithLaneId): + return path_point.point + else: + raise TypeError("invalid path_point argument type") - s_arr = [] - ds = 0.0 - s_sum = 0.0 + def CalcDistance(self, point0, point1): + p0 = self.getPoint(point0).pose.position + p1 = self.getPoint(point1).pose.position + dx = p1.x - p0.x + dy = p1.y - p0.y + return np.sqrt(dx**2 + dy**2) - closest_id = self.calcClosestPathWLid(traj) - for i in range(1, closest_id): - p0 = traj.points[i - 1].point - p1 = traj.points[i].point - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum -= ds - - s_arr.append(s_sum) - for i in range(1, len(traj.points)): - p0 = traj.points[i - 1].point - p1 = traj.points[i].point - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum += ds - s_arr.append(s_sum) - return s_arr - - def CalcArcLengthPath(self, traj): + def CalcArcLength(self, traj): if len(traj.points) == 0: return s_arr = [] - ds = 0.0 s_sum = 0.0 - closest_id = self.calcClosestPath(traj) + closest_id = self.calcClosestIndex(traj) for i in range(1, closest_id): - p0 = traj.points[i - 1] - p1 = traj.points[i] - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum -= ds - + s_sum -= self.CalcDistance( + self.getPoint(traj.points[i - 1]), self.getPoint(traj.points[i]) + ) s_arr.append(s_sum) for i in range(1, len(traj.points)): - p0 = traj.points[i - 1] - p1 = traj.points[i] - dx = p1.pose.position.x - p0.pose.position.x - dy = p1.pose.position.y - p0.pose.position.y - ds = np.sqrt(dx**2 + dy**2) - s_sum += ds + s_sum += self.CalcDistance( + self.getPoint(traj.points[i - 1]), self.getPoint(traj.points[i]) + ) s_arr.append(s_sum) return s_arr def ToVelList(self, traj): - v_list = [] - for p in traj.points: - v_list.append(p.longitudinal_velocity_mps) - return v_list - - def ToVelListPathWLid(self, traj): - v_list = [] - for p in traj.points: - v_list.append(p.point.longitudinal_velocity_mps) - return v_list - - def ToVelListPath(self, traj): - v_list = [] - for p in traj.points: - v_list.append(p.longitudinal_velocity_mps) - return v_list + return [self.getPoint(p).longitudinal_velocity_mps for p in traj.points] def CalcAcceleration(self, traj): a_arr = [] @@ -606,8 +496,7 @@ def setPlotTrajectory(self): return self.im0, self.im1, self.im2, self.im3, self.im4 def plotTrajectory(self, data): - print("plot called") - # self.updatePose(PATH_ORIGIN_FRAME, SELF_POSE_FRAME) + self.get_logger().info("plot called") # copy trajectory_raw = self.trajectory_raw @@ -663,40 +552,16 @@ def plotTrajectory(self, data): return self.im0, self.im1, self.im2, self.im3, self.im4 - def calcClosestPath(self, path): - closest = -1 - min_dist_squared = 1.0e10 - for i in range(0, len(path.points)): - dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].pose) - if dist_sq < min_dist_squared: - min_dist_squared = dist_sq - closest = i - return closest - - def calcClosestPathWLid(self, path): - closest = -1 - min_dist_squared = 1.0e10 - for i in range(0, len(path.points)): - dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].point.pose) - if dist_sq < min_dist_squared: - min_dist_squared = dist_sq - closest = i - return closest - - def calcClosestTrajectory(self, path): - closest = -1 - min_dist_squared = 1.0e10 - for i in range(0, len(path.points)): - dist_sq = self.calcSquaredDist2d(self.self_pose, path.points[i].pose) - if dist_sq < min_dist_squared: - min_dist_squared = dist_sq - closest = i - return closest - - def calcSquaredDist2d(self, p1, p2): - dx = p1.position.x - p2.position.x - dy = p1.position.y - p2.position.y - return dx * dx + dy * dy + def calcClosestIndex(self, path): + points = np.array( + [ + [self.getPoint(p).pose.position.x, self.getPoint(p).pose.position.y] + for p in path.points + ] + ) + self_pose = np.array([self.self_pose.position.x, self.self_pose.position.y]) + dists_squared = np.sum((points - self_pose) ** 2, axis=1) + return np.argmin(dists_squared) def closeFigure(self): plt.close(self.fig) 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/planning_validator/src/debug_marker.cpp b/planning/planning_validator/src/debug_marker.cpp index 89614f5c74f5c..1a0793957402a 100644 --- a/planning/planning_validator/src/debug_marker.cpp +++ b/planning/planning_validator/src/debug_marker.cpp @@ -14,8 +14,8 @@ #include "planning_validator/debug_marker.hpp" -#include -#include +#include +#include #include #include diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index 7bb37fdcdd467..db4510c2e5d0d 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -17,7 +17,7 @@ #include "planning_validator/utils.hpp" #include -#include +#include #include #include diff --git a/planning/planning_validator/src/utils.cpp b/planning/planning_validator/src/utils.cpp index e4c882cfaedb9..5a90e950bb616 100644 --- a/planning/planning_validator/src/utils.cpp +++ b/planning/planning_validator/src/utils.cpp @@ -15,7 +15,7 @@ #include "planning_validator/utils.hpp" #include -#include +#include #include #include diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index a797edaa34556..6bd9756098558 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -16,9 +16,7 @@ #define ROUTE_HANDLER__ROUTE_HANDLER_HPP_ #include -#include #include -#include #include #include @@ -122,7 +120,8 @@ class RouteHandler * @return vector of lanelet having same direction if true */ boost::optional getRightLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; /** * @brief Check if same-direction lane is available at the left side of the lanelet @@ -132,7 +131,8 @@ class RouteHandler * @return vector of lanelet having same direction if true */ boost::optional getLeftLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; lanelet::ConstLanelets getNextLanelets(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getPreviousLanelets(const lanelet::ConstLanelet & lanelet) const; @@ -195,7 +195,8 @@ class RouteHandler * @return vector of lanelet having same direction if true */ lanelet::ConstLanelet getMostRightLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; /** * @brief Check if same-direction lane is available at the left side of the lanelet @@ -205,7 +206,8 @@ class RouteHandler * @return vector of lanelet having same direction if true */ lanelet::ConstLanelet getMostLeftLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; /** * @brief Searches the furthest linestring to the right side of the lanelet diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml index ef2051d1b244a..6304e21932676 100644 --- a/planning/route_handler/package.xml +++ b/planning/route_handler/package.xml @@ -24,7 +24,6 @@ autoware_planning_msgs geometry_msgs lanelet2_extension - motion_utils rclcpp rclcpp_components tf2_ros diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 91f86ed0e454f..3d9dd8fe7a64a 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -528,7 +529,14 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceAfter( while (rclcpp::ok() && length < min_length) { lanelet::ConstLanelet next_lanelet; if (!getNextLaneletWithinRoute(current_lanelet, &next_lanelet)) { - break; + if (only_route_lanes) { + break; + } + const auto next_lanes = getNextLanelets(current_lanelet); + if (next_lanes.empty()) { + break; + } + next_lanelet = next_lanes.front(); } // loop check if (lanelet.id() == next_lanelet.id()) { @@ -556,7 +564,14 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceUpTo( while (rclcpp::ok() && length < min_length) { lanelet::ConstLanelets candidate_lanelets; if (!getPreviousLaneletsWithinRoute(current_lanelet, &candidate_lanelets)) { - break; + if (only_route_lanes) { + break; + } + const auto prev_lanes = getPreviousLanelets(current_lanelet); + if (prev_lanes.empty()) { + break; + } + candidate_lanelets = prev_lanes; } // loop check if (std::any_of( @@ -954,7 +969,8 @@ bool RouteHandler::isBijectiveConnection( } boost::optional RouteHandler::getRightLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const { // right road lanelet of shoulder lanelet if (isShoulderLanelet(lanelet)) { @@ -966,6 +982,14 @@ boost::optional RouteHandler::getRightLanelet( return boost::none; } + // right shoulder lanelet + if (get_shoulder_lane) { + lanelet::ConstLanelet right_shoulder_lanelet; + if (getRightShoulderLanelet(lanelet, &right_shoulder_lanelet)) { + return right_shoulder_lanelet; + } + } + // routable lane const auto & right_lane = routing_graph_ptr_->right(lanelet); if (right_lane) { @@ -1026,7 +1050,8 @@ bool RouteHandler::getLeftLaneletWithinRoute( } boost::optional RouteHandler::getLeftLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const { // left road lanelet of shoulder lanelet if (isShoulderLanelet(lanelet)) { @@ -1038,6 +1063,14 @@ boost::optional RouteHandler::getLeftLanelet( return boost::none; } + // left shoulder lanelet + if (get_shoulder_lane) { + lanelet::ConstLanelet left_shoulder_lanelet; + if (getLeftShoulderLanelet(lanelet, &left_shoulder_lanelet)) { + return left_shoulder_lanelet; + } + } + // routable lane const auto & left_lane = routing_graph_ptr_->left(lanelet); if (left_lane) { @@ -1213,26 +1246,28 @@ lanelet::Lanelets RouteHandler::getLeftOppositeLanelets(const lanelet::ConstLane } lanelet::ConstLanelet RouteHandler::getMostRightLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const { // recursively compute the width of the lanes - const auto & same = getRightLanelet(lanelet, enable_same_root); + const auto & same = getRightLanelet(lanelet, enable_same_root, get_shoulder_lane); if (same) { - return getMostRightLanelet(same.get(), enable_same_root); + return getMostRightLanelet(same.get(), enable_same_root, get_shoulder_lane); } return lanelet; } lanelet::ConstLanelet RouteHandler::getMostLeftLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const { // recursively compute the width of the lanes - const auto & same = getLeftLanelet(lanelet, enable_same_root); + const auto & same = getLeftLanelet(lanelet, enable_same_root, get_shoulder_lane); if (same) { - return getMostLeftLanelet(same.get(), enable_same_root); + return getMostLeftLanelet(same.get(), enable_same_root, get_shoulder_lane); } return lanelet; diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp index 30d9a77f31fad..7cf88cb75e13a 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp @@ -18,7 +18,10 @@ #include "path_sampler/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "sampler_common/structures.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" + +#include #include #include diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp index b1c6ec61db845..a9002c8cf8a9f 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp @@ -15,13 +15,11 @@ #ifndef PATH_SAMPLER__NODE_HPP_ #define PATH_SAMPLER__NODE_HPP_ -#include "motion_utils/motion_utils.hpp" #include "path_sampler/common_structs.hpp" #include "path_sampler/parameters.hpp" #include "path_sampler/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "sampler_common/transform/spline_transform.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include diff --git a/planning/sampling_based_planner/path_sampler/src/node.cpp b/planning/sampling_based_planner/path_sampler/src/node.cpp index 588f29b06c40d..aa037755bbedd 100644 --- a/planning/sampling_based_planner/path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/path_sampler/src/node.cpp @@ -15,6 +15,7 @@ #include "path_sampler/node.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" +#include "motion_utils/marker/marker_helper.hpp" #include "path_sampler/path_generation.hpp" #include "path_sampler/prepare_inputs.hpp" #include "path_sampler/utils/geometry_utils.hpp" @@ -23,6 +24,8 @@ #include "sampler_common/constraints/hard_constraint.hpp" #include "sampler_common/constraints/soft_constraint.hpp" +#include + #include #include diff --git a/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp b/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp index c17f3479932f3..fd52764950ca9 100644 --- a/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp +++ b/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp @@ -18,6 +18,7 @@ #include "path_sampler/utils/geometry_utils.hpp" #include "sampler_common/structures.hpp" #include "sampler_common/transform/spline_transform.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include diff --git a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp b/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp index ec41f3032e150..db55e5f8557e4 100644 --- a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp +++ b/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp @@ -14,7 +14,8 @@ #include "path_sampler/utils/trajectory_utils.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/resample/resample.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" #include "path_sampler/utils/geometry_utils.hpp" #include "tf2/utils.h" diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp index 5798c19d0d563..1a700d36deaaf 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp @@ -15,6 +15,7 @@ #define STATIC_CENTERLINE_OPTIMIZER__TYPE_ALIAS_HPP_ #include "route_handler/route_handler.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" diff --git a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp index b2e136fd3916b..8c3b84854a6d0 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp @@ -18,17 +18,21 @@ #include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" #include "map_loader/lanelet2_map_loader_node.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" #include "static_centerline_optimizer/msg/points_with_lane_id.hpp" #include "static_centerline_optimizer/successive_trajectory_optimizer_node.hpp" #include "static_centerline_optimizer/type_alias.hpp" #include "static_centerline_optimizer/utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include +#include #include +#include + #include #include #include diff --git a/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp b/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp index 325c257e228f9..ab147ab1f4f74 100644 --- a/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/successive_trajectory_optimizer_node.cpp @@ -14,9 +14,11 @@ #include "static_centerline_optimizer/successive_trajectory_optimizer_node.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/resample/resample.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "obstacle_avoidance_planner/utils/trajectory_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include diff --git a/planning/static_centerline_optimizer/src/utils.cpp b/planning/static_centerline_optimizer/src/utils.cpp index 691ae7960968c..7871eb9e95163 100644 --- a/planning/static_centerline_optimizer/src/utils.cpp +++ b/planning/static_centerline_optimizer/src/utils.cpp @@ -16,8 +16,8 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" namespace static_centerline_optimizer { namespace 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..f31bb7ddee331 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp @@ -17,10 +17,8 @@ #include "surround_obstacle_checker/debug_marker.hpp" -#include +#include #include -#include -#include #include #include @@ -32,12 +30,15 @@ #include #include +#include + #include #include #include #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..d3d73a89e0a94 100644 --- a/planning/surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/surround_obstacle_checker/src/debug_marker.cpp @@ -14,8 +14,9 @@ #include "surround_obstacle_checker/debug_marker.hpp" -#include -#include +#include +#include +#include #ifdef ROS_DISTRO_GALACTIC #include #else @@ -26,6 +27,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 +60,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 +114,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 +239,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 +257,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..ab542bdfa73ed 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -15,6 +15,9 @@ #include "surround_obstacle_checker/node.hpp" #include +#include +#include +#include #include #include @@ -47,6 +50,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 +124,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 +152,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 +211,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 +223,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 +284,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 +308,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 +338,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 +392,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 +412,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 +429,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 +445,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 +460,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 +467,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 +501,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/sensing/imu_corrector/package.xml b/sensing/imu_corrector/package.xml index 7466d158da2df..3dff79bbf9f7f 100644 --- a/sensing/imu_corrector/package.xml +++ b/sensing/imu_corrector/package.xml @@ -13,6 +13,7 @@ ament_cmake_auto autoware_cmake + diagnostic_updater rclcpp rclcpp_components sensor_msgs diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp index 7c302eec20431..5829277335433 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp @@ -36,7 +36,12 @@ void FasterVoxelGridDownsampleFilter::set_field_offsets(const PointCloud2ConstPt x_offset_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; y_offset_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; z_offset_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; - intensity_offset_ = input->fields[pcl::getFieldIndex(*input, "intensity")].offset; + int intensity_index = pcl::getFieldIndex(*input, "intensity"); + if (intensity_index != -1) { + intensity_offset_ = input->fields[intensity_index].offset; + } else { + intensity_offset_ = z_offset_ + sizeof(float); + } offset_initialized_ = true; } diff --git a/sensing/radar_static_pointcloud_filter/CMakeLists.txt b/sensing/radar_static_pointcloud_filter/CMakeLists.txt index 1f08f7842cfdf..ecd33d3166b19 100644 --- a/sensing/radar_static_pointcloud_filter/CMakeLists.txt +++ b/sensing/radar_static_pointcloud_filter/CMakeLists.txt @@ -27,4 +27,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/sensing/radar_static_pointcloud_filter/config/radar_static_pointcloud_filter.param.yaml b/sensing/radar_static_pointcloud_filter/config/radar_static_pointcloud_filter.param.yaml new file mode 100644 index 0000000000000..a429f7a065ffe --- /dev/null +++ b/sensing/radar_static_pointcloud_filter/config/radar_static_pointcloud_filter.param.yaml @@ -0,0 +1,17 @@ +# Copyright 2023 Foxconn, 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. + +/**: + ros__parameters: + doppler_velocity_sd: 4.0 diff --git a/sensing/radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml b/sensing/radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml index 08fdeee74f15a..b9491227837c5 100644 --- a/sensing/radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml +++ b/sensing/radar_static_pointcloud_filter/launch/radar_static_pointcloud_filter.launch.xml @@ -3,13 +3,14 @@ - + + - + diff --git a/sensing/radar_static_pointcloud_filter/schema/radar_static_pointcloud_filter.schema.json b/sensing/radar_static_pointcloud_filter/schema/radar_static_pointcloud_filter.schema.json new file mode 100644 index 0000000000000..65340d889f62e --- /dev/null +++ b/sensing/radar_static_pointcloud_filter/schema/radar_static_pointcloud_filter.schema.json @@ -0,0 +1,31 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Radar Static Pointcloud Filter Node", + "type": "object", + "definitions": { + "radar_static_pointcloud_filter": { + "type": "object", + "properties": { + "doppler_velocity_sd": { + "type": "number", + "default": "4.0", + "minimum": 0.0, + "description": "Standard deviation for radar doppler velocity. [m/s]" + } + }, + "required": ["doppler_velocity_sd"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/radar_static_pointcloud_filter" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp index c3550b0338675..772dcef753c11 100644 --- a/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp +++ b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2022 TIER IV, Inc. +// Copyright 2022-2023 Foxconn, 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. @@ -93,7 +93,7 @@ RadarStaticPointcloudFilterNode::RadarStaticPointcloudFilterNode( std::bind(&RadarStaticPointcloudFilterNode::onSetParam, this, _1)); // Node Parameter - node_param_.doppler_velocity_sd = declare_parameter("doppler_velocity_sd", 2.0); + node_param_.doppler_velocity_sd = declare_parameter("doppler_velocity_sd"); // Subscriber transform_listener_ = std::make_shared(this); 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 5c8a7eabdab86..fe28d063eedd3 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,6 +15,7 @@ #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/geometry/geometry.hpp" @@ -22,6 +23,11 @@ #include "tier4_autoware_utils/ros/update_param.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" +#include +#include + +#include +#include #include #include @@ -47,13 +53,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(); @@ -68,6 +75,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 @@ -82,11 +102,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( @@ -253,6 +277,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_) { @@ -260,16 +322,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_) { @@ -278,7 +345,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); @@ -310,6 +377,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 @@ -346,7 +426,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; @@ -358,11 +439,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 (