diff --git a/common/autoware_ad_api_specs/package.xml b/common/autoware_ad_api_specs/package.xml index d4d82faf4ee78..0d14ccff758b0 100644 --- a/common/autoware_ad_api_specs/package.xml +++ b/common/autoware_ad_api_specs/package.xml @@ -5,8 +5,6 @@ 0.0.0 The autoware_ad_api_specs package Takagi, Isamu - yabuta - Kah Hooi Tan Ryohsuke Mitsudome Apache License 2.0 diff --git a/common/component_interface_specs/package.xml b/common/component_interface_specs/package.xml index 93f77c651869d..67279d0ae2b7f 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -5,8 +5,6 @@ 0.0.0 The component_interface_specs package Takagi, Isamu - yabuta - Kah Hooi Tan Yukihiro Saito Apache License 2.0 diff --git a/common/component_interface_tools/package.xml b/common/component_interface_tools/package.xml index ebf0684d0066c..cff1829473e86 100644 --- a/common/component_interface_tools/package.xml +++ b/common/component_interface_tools/package.xml @@ -5,8 +5,6 @@ 0.1.0 The component_interface_tools package Takagi, Isamu - yabuta - Kah Hooi Tan Apache License 2.0 ament_cmake_auto diff --git a/common/component_interface_utils/package.xml b/common/component_interface_utils/package.xml index dc82fda3f5c64..7a6a6d12880ad 100755 --- a/common/component_interface_utils/package.xml +++ b/common/component_interface_utils/package.xml @@ -5,8 +5,6 @@ 0.0.0 The component_interface_utils package Takagi, Isamu - yabuta - Kah Hooi Tan Yukihiro Saito Apache License 2.0 diff --git a/common/path_distance_calculator/package.xml b/common/path_distance_calculator/package.xml index b5e770a0ea624..e796bbd0d20f7 100644 --- a/common/path_distance_calculator/package.xml +++ b/common/path_distance_calculator/package.xml @@ -5,8 +5,6 @@ 0.0.0 The path_distance_calculator package Takagi, Isamu - yabuta - Kah Hooi Tan Apache License 2.0 ament_cmake diff --git a/common/tier4_api_utils/package.xml b/common/tier4_api_utils/package.xml index 091f512480150..d05b810caf904 100644 --- a/common/tier4_api_utils/package.xml +++ b/common/tier4_api_utils/package.xml @@ -5,8 +5,6 @@ 0.0.0 The tier4_api_utils package Takagi, Isamu - yabuta - Kah Hooi Tan Apache License 2.0 ament_cmake_auto diff --git a/common/tier4_datetime_rviz_plugin/package.xml b/common/tier4_datetime_rviz_plugin/package.xml index 6dc0c09b32a3b..82e1e82c61ba2 100644 --- a/common/tier4_datetime_rviz_plugin/package.xml +++ b/common/tier4_datetime_rviz_plugin/package.xml @@ -5,8 +5,6 @@ 0.0.0 The tier4_datetime_rviz_plugin package Takagi, Isamu - yabuta - Kah Hooi Tan Apache License 2.0 ament_cmake_auto diff --git a/common/tier4_debug_rviz_plugin/CMakeLists.txt b/common/tier4_debug_rviz_plugin/CMakeLists.txt index 05cf35b93ef9f..02e65ad759ed6 100644 --- a/common/tier4_debug_rviz_plugin/CMakeLists.txt +++ b/common/tier4_debug_rviz_plugin/CMakeLists.txt @@ -12,7 +12,9 @@ add_definitions(-DQT_NO_KEYWORDS) ament_auto_add_library(tier4_debug_rviz_plugin SHARED include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp include/tier4_debug_rviz_plugin/jsk_overlay_utils.hpp + include/tier4_debug_rviz_plugin/string_stamped.hpp src/float32_multi_array_stamped_pie_chart.cpp + src/string_stamped.cpp src/jsk_overlay_utils.cpp ) diff --git a/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png b/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png differ diff --git a/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp new file mode 100644 index 0000000000000..0960875d7eee2 --- /dev/null +++ b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp @@ -0,0 +1,107 @@ +// 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 TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_ +#define TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_ + +#include +#include + +#ifndef Q_MOC_RUN +#include "tier4_debug_rviz_plugin/jsk_overlay_utils.hpp" + +#include +#include +#include +#include + +#endif + +#include + +namespace rviz_plugins +{ +class StringStampedOverlayDisplay +: public rviz_common::RosTopicDisplay + +{ + Q_OBJECT + +public: + StringStampedOverlayDisplay(); + ~StringStampedOverlayDisplay() 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 tier4_debug_msgs::msg::StringStamped::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_; + tier4_debug_msgs::msg::StringStamped::ConstSharedPtr last_msg_ptr_; +}; +} // namespace rviz_plugins + +#endif // TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_ diff --git a/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml b/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml index c1158465e1cf1..e18900afc8d84 100644 --- a/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml @@ -4,4 +4,9 @@ base_class_type="rviz_common::Display"> Display drivable area of tier4_debug_msgs::msg::Float32MultiArrayStamped + + Display drivable area of tier4_debug_msgs::msg::StringStamped + diff --git a/common/tier4_debug_rviz_plugin/src/string_stamped.cpp b/common/tier4_debug_rviz_plugin/src/string_stamped.cpp new file mode 100644 index 0000000000000..538dc0cbe7069 --- /dev/null +++ b/common/tier4_debug_rviz_plugin/src/string_stamped.cpp @@ -0,0 +1,198 @@ +// 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 "tier4_debug_rviz_plugin/string_stamped.hpp" + +#include "tier4_debug_rviz_plugin/jsk_overlay_utils.hpp" + +#include +#include + +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ +StringStampedOverlayDisplay::StringStampedOverlayDisplay() +{ + 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", 15, "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); +} + +StringStampedOverlayDisplay::~StringStampedOverlayDisplay() +{ + if (initialized()) { + overlay_->hide(); + } +} + +void StringStampedOverlayDisplay::onInitialize() +{ + RTDClass::onInitialize(); + + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "StringOverlayDisplayObject" << count++; + auto logger = context_->getRosNodeAbstraction().lock()->get_raw_node()->get_logger(); + overlay_.reset(new jsk_rviz_plugins::OverlayObject(scene_manager_, logger, 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 StringStampedOverlayDisplay::onEnable() +{ + subscribe(); + overlay_->show(); +} + +void StringStampedOverlayDisplay::onDisable() +{ + unsubscribe(); + reset(); + overlay_->hide(); +} + +void StringStampedOverlayDisplay::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + std::lock_guard message_lock(mutex_); + if (!last_msg_ptr_) { + return; + } + + // 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); + + // 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, + last_msg_ptr_->data.c_str()); + painter.end(); + updateVisualization(); +} + +void StringStampedOverlayDisplay::processMessage( + const tier4_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) +{ + if (!isEnabled()) { + return; + } + + { + std::lock_guard message_lock(mutex_); + last_msg_ptr_ = msg_ptr; + } + + queueRender(); +} + +void StringStampedOverlayDisplay::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::StringStampedOverlayDisplay, rviz_common::Display) diff --git a/common/tier4_localization_rviz_plugin/package.xml b/common/tier4_localization_rviz_plugin/package.xml index e103db19f1d43..f2482b2d45a3b 100644 --- a/common/tier4_localization_rviz_plugin/package.xml +++ b/common/tier4_localization_rviz_plugin/package.xml @@ -5,8 +5,6 @@ 0.1.0 The tier4_localization_rviz_plugin package Takagi, Isamu - yabuta - Kah Hooi Tan Takamasa Horibe Apache License 2.0 diff --git a/launch/tier4_autoware_api_launch/package.xml b/launch/tier4_autoware_api_launch/package.xml index 4d41ad0ac4a83..c55f1c8038e83 100644 --- a/launch/tier4_autoware_api_launch/package.xml +++ b/launch/tier4_autoware_api_launch/package.xml @@ -5,8 +5,6 @@ 0.0.0 The tier4_autoware_api_launch package Takagi, Isamu - yabuta - Kah Hooi Tan Ryohsuke Mitsudome Apache License 2.0 diff --git a/perception/tensorrt_yolo/config/yolov3.param.yaml b/perception/tensorrt_yolo/config/yolov3.param.yaml index 5ce50f3acfa96..d97505dae5020 100644 --- a/perception/tensorrt_yolo/config/yolov3.param.yaml +++ b/perception/tensorrt_yolo/config/yolov3.param.yaml @@ -3,7 +3,7 @@ num_anchors: 3 anchors: [116.0, 90.0, 156.0, 198.0, 373.0, 326.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 10.0, 13.0, 16.0, 30.0, 33.0, 23.0] scale_x_y: [1.0, 1.0, 1.0] - score_threshold: 0.1 + score_thresh: 0.1 iou_thresh: 0.45 detections_per_im: 100 use_darknet_layer: true diff --git a/perception/tensorrt_yolo/config/yolov4-tiny.param.yaml b/perception/tensorrt_yolo/config/yolov4-tiny.param.yaml index 2ee53ee68f764..8071da9dcac06 100644 --- a/perception/tensorrt_yolo/config/yolov4-tiny.param.yaml +++ b/perception/tensorrt_yolo/config/yolov4-tiny.param.yaml @@ -3,7 +3,7 @@ num_anchors: 3 anchors: [81.0, 82.0, 135.0, 169.0, 344.0, 319.0, 23.0, 27.0, 37.0, 58.0, 81.0, 82.0] scale_x_y: [1.05, 1.05] - score_threshold: 0.1 + score_thresh: 0.1 iou_thresh: 0.45 detections_per_im: 100 use_darknet_layer: true diff --git a/perception/tensorrt_yolo/config/yolov4.param.yaml b/perception/tensorrt_yolo/config/yolov4.param.yaml index 6122af667c470..9edc0fc6ce708 100644 --- a/perception/tensorrt_yolo/config/yolov4.param.yaml +++ b/perception/tensorrt_yolo/config/yolov4.param.yaml @@ -3,7 +3,7 @@ num_anchors: 3 anchors: [12.0, 16.0, 19.0, 36.0, 40.0, 28.0, 36.0, 75.0, 76.0, 55.0, 72.0, 146.0, 142.0, 110.0, 192.0, 243.0, 459.0, 401.0] scale_x_y: [1.2, 1.1, 1.05] - score_threshold: 0.1 + score_thresh: 0.1 iou_thresh: 0.45 detections_per_im: 100 use_darknet_layer: true diff --git a/perception/tensorrt_yolo/config/yolov5l.param.yaml b/perception/tensorrt_yolo/config/yolov5l.param.yaml index 8a709d057594b..67ae862fc33af 100644 --- a/perception/tensorrt_yolo/config/yolov5l.param.yaml +++ b/perception/tensorrt_yolo/config/yolov5l.param.yaml @@ -3,7 +3,7 @@ num_anchors: 3 anchors: [10.0, 13.0, 16.0, 30.0, 33.0, 23.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 116.0, 90.0, 156.0, 198.0, 373.0, 326.0] scale_x_y: [1.0, 1.0, 1.0] - score_threshold: 0.1 + score_thresh: 0.1 iou_thresh: 0.45 detections_per_im: 100 use_darknet_layer: false diff --git a/perception/tensorrt_yolo/config/yolov5m.param.yaml b/perception/tensorrt_yolo/config/yolov5m.param.yaml index 8a709d057594b..67ae862fc33af 100644 --- a/perception/tensorrt_yolo/config/yolov5m.param.yaml +++ b/perception/tensorrt_yolo/config/yolov5m.param.yaml @@ -3,7 +3,7 @@ num_anchors: 3 anchors: [10.0, 13.0, 16.0, 30.0, 33.0, 23.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 116.0, 90.0, 156.0, 198.0, 373.0, 326.0] scale_x_y: [1.0, 1.0, 1.0] - score_threshold: 0.1 + score_thresh: 0.1 iou_thresh: 0.45 detections_per_im: 100 use_darknet_layer: false diff --git a/perception/tensorrt_yolo/config/yolov5s.param.yaml b/perception/tensorrt_yolo/config/yolov5s.param.yaml index 8a709d057594b..67ae862fc33af 100644 --- a/perception/tensorrt_yolo/config/yolov5s.param.yaml +++ b/perception/tensorrt_yolo/config/yolov5s.param.yaml @@ -3,7 +3,7 @@ num_anchors: 3 anchors: [10.0, 13.0, 16.0, 30.0, 33.0, 23.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 116.0, 90.0, 156.0, 198.0, 373.0, 326.0] scale_x_y: [1.0, 1.0, 1.0] - score_threshold: 0.1 + score_thresh: 0.1 iou_thresh: 0.45 detections_per_im: 100 use_darknet_layer: false diff --git a/perception/tensorrt_yolo/config/yolov5x.param.yaml b/perception/tensorrt_yolo/config/yolov5x.param.yaml index 8a709d057594b..67ae862fc33af 100644 --- a/perception/tensorrt_yolo/config/yolov5x.param.yaml +++ b/perception/tensorrt_yolo/config/yolov5x.param.yaml @@ -3,7 +3,7 @@ num_anchors: 3 anchors: [10.0, 13.0, 16.0, 30.0, 33.0, 23.0, 30.0, 61.0, 62.0, 45.0, 59.0, 119.0, 116.0, 90.0, 156.0, 198.0, 373.0, 326.0] scale_x_y: [1.0, 1.0, 1.0] - score_threshold: 0.1 + score_thresh: 0.1 iou_thresh: 0.45 detections_per_im: 100 use_darknet_layer: false 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 f6f95ae3b5e82..5e773eba96aee 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 @@ -46,6 +46,7 @@ using SceneModulePtr = std::shared_ptr; using SceneModuleManagerPtr = std::shared_ptr; using DebugPublisher = tier4_autoware_utils::DebugPublisher; using DebugDoubleMsg = tier4_debug_msgs::msg::Float64Stamped; +using DebugStringMsg = tier4_debug_msgs::msg::StringStamped; enum Action { ADD = 0, @@ -456,6 +457,8 @@ class PlannerManager std::unique_ptr debug_publisher_ptr_; + std::unique_ptr state_publisher_ptr_; + pluginlib::ClassLoader plugin_loader_; mutable rclcpp::Logger logger_; diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index a1d0a82be38b1..cbcec2e3095d3 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -40,6 +40,7 @@ PlannerManager::PlannerManager( { processing_time_.emplace("total_time", 0.0); debug_publisher_ptr_ = std::make_unique(&node, "~/debug"); + state_publisher_ptr_ = std::make_unique(&node, "~/debug"); } void PlannerManager::launchScenePlugin(rclcpp::Node & node, const std::string & name) @@ -908,10 +909,6 @@ void PlannerManager::resetRootLanelet(const std::shared_ptr & data) void PlannerManager::print() const { - if (!verbose_) { - return; - } - const auto get_status = [](const auto & m) { return magic_enum::enum_name(m->getCurrentStatus()); }; @@ -961,6 +958,12 @@ void PlannerManager::print() const << std::setw(21); } + state_publisher_ptr_->publish("internal_state", string_stream.str()); + + if (!verbose_) { + return; + } + RCLCPP_INFO_STREAM(logger_, string_stream.str()); } diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index f00ad01efb0d9..f7ce218cf3ccf 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -37,19 +37,21 @@ StopSpeedExceeded createStopSpeedExceededMsg( } tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray( - const rclcpp::Time & current_time, const geometry_msgs::msg::Pose & stop_pose, + const PlannerData & planner_data, const geometry_msgs::msg::Pose & stop_pose, const StopObstacle & stop_obstacle) { // create header std_msgs::msg::Header header; header.frame_id = "map"; - header.stamp = current_time; + header.stamp = planner_data.current_time; // create stop factor StopFactor stop_factor; stop_factor.stop_pose = stop_pose; geometry_msgs::msg::Point stop_factor_point = stop_obstacle.collision_point; stop_factor_point.z = stop_pose.position.z; + stop_factor.dist_to_stop_pose = motion_utils::calcSignedArcLength( + planner_data.traj_points, planner_data.ego_pose.position, stop_pose.position); stop_factor.stop_factor_points.emplace_back(stop_factor_point); // create stop reason stamped @@ -359,7 +361,7 @@ std::vector PlannerInterface::generateStopTrajectory( // Publish Stop Reason const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose; const auto stop_reasons_msg = - makeStopReasonArray(planner_data.current_time, stop_pose, *closest_stop_obstacle); + makeStopReasonArray(planner_data, stop_pose, *closest_stop_obstacle); stop_reasons_pub_->publish(stop_reasons_msg); velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose)); diff --git a/system/component_state_monitor/package.xml b/system/component_state_monitor/package.xml index 60d0fd0577ef0..16903fa216174 100644 --- a/system/component_state_monitor/package.xml +++ b/system/component_state_monitor/package.xml @@ -5,8 +5,6 @@ 0.1.0 The component_state_monitor package Takagi, Isamu - yabuta - Kah Hooi Tan Apache License 2.0 ament_cmake_auto diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 6a9a22f39cdc4..4e691f7ab3192 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -5,8 +5,6 @@ 0.1.0 The default_ad_api package Takagi, Isamu - Kah Hooi Tan - yabuta Ryohsuke Mitsudome Yukihiro Saito Apache License 2.0 diff --git a/system/default_ad_api_helpers/ad_api_adaptors/package.xml b/system/default_ad_api_helpers/ad_api_adaptors/package.xml index e54a5faa6e1b7..785ff58db1f81 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/package.xml +++ b/system/default_ad_api_helpers/ad_api_adaptors/package.xml @@ -5,8 +5,6 @@ 0.1.0 The ad_api_adaptors package Takagi, Isamu - Kah Hooi Tan - yabuta Ryohsuke Mitsudome Yukihiro Saito Apache License 2.0 diff --git a/system/default_ad_api_helpers/ad_api_visualizers/package.xml b/system/default_ad_api_helpers/ad_api_visualizers/package.xml index 95961769813cd..75fd09a1335b1 100644 --- a/system/default_ad_api_helpers/ad_api_visualizers/package.xml +++ b/system/default_ad_api_helpers/ad_api_visualizers/package.xml @@ -5,8 +5,6 @@ 0.1.0 The ad_api_visualizers package Takagi, Isamu - Kah Hooi Tan - yabuta Ryohsuke Mitsudome Yukihiro Saito Apache License 2.0 diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/package.xml b/system/default_ad_api_helpers/automatic_pose_initializer/package.xml index 10f4abfe446fb..be192c3dade4b 100644 --- a/system/default_ad_api_helpers/automatic_pose_initializer/package.xml +++ b/system/default_ad_api_helpers/automatic_pose_initializer/package.xml @@ -5,8 +5,6 @@ 0.1.0 The automatic_pose_initializer package Takagi, Isamu - Kah Hooi Tan - yabuta Ryohsuke Mitsudome Yukihiro Saito Apache License 2.0