Merge pull request #1593 from tier4/hotfix/v3.0.2/output-internal-info

fix: cherry pick PRs to fix invalid access

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -590,8 +590,12 @@ double calcLateralOffset(
     return std::nan("");
   }
 
-  const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx));
-  const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx + 1));
+  const auto p_indices = overlap_removed_points.size() - 2;
+  const auto p_front_idx = (p_indices > seg_idx) ? seg_idx : p_indices;
+  const auto p_back_idx = p_front_idx + 1;
+
+  const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(p_front_idx));
+  const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(p_back_idx));
 
   const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0};
   const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0};

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
 )
 

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 <memory>
+#include <mutex>
+
+#ifndef Q_MOC_RUN
+#include "tier4_debug_rviz_plugin/jsk_overlay_utils.hpp"
+
+#include <rviz_common/properties/color_property.hpp>
+#include <rviz_common/properties/float_property.hpp>
+#include <rviz_common/properties/int_property.hpp>
+#include <rviz_common/ros_topic_display.hpp>
+
+#endif
+
+#include <tier4_debug_msgs/msg/string_stamped.hpp>
+
+namespace rviz_plugins
+{
+class StringStampedOverlayDisplay
+: public rviz_common::RosTopicDisplay<tier4_debug_msgs::msg::StringStamped>
+
+{
+  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_

common/tier4_debug_rviz_plugin/plugins/plugin_description.xml

@@ -4,4 +4,9 @@
     base_class_type="rviz_common::Display">
     <description>Display drivable area of tier4_debug_msgs::msg::Float32MultiArrayStamped</description>
   </class>
+  <class name="rviz_plugins/StringStampedOverlayDisplay"
+    type="rviz_plugins::StringStampedOverlayDisplay"
+    base_class_type="rviz_common::Display">
+    <description>Display drivable area of tier4_debug_msgs::msg::StringStamped</description>
+  </class>
 </library>

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 <QPainter>
+#include <rviz_common/uniform_string_stream.hpp>
+
+#include <X11/Xlib.h>
+
+#include <algorithm>
+#include <optional>
+#include <string>
+#include <vector>
+
+namespace rviz_plugins
+{
+StringStampedOverlayDisplay::StringStampedOverlayDisplay()
+{
+  const Screen * screen_info = DefaultScreenOfDisplay(XOpenDisplay(NULL));
+
+  constexpr float hight_4k = 2160.0;
+  const float scale = static_cast<float>(screen_info->height) / hight_4k;
+  const auto left = static_cast<int>(std::round(1024 * scale));
+  const auto top = static_cast<int>(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<std::mutex> 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<int>(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<std::mutex> 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/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(rviz_plugins::StringStampedOverlayDisplay, rviz_common::Display)

planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

@@ -46,6 +46,7 @@ using SceneModulePtr = std::shared_ptr<SceneModuleInterface>;
 using SceneModuleManagerPtr = std::shared_ptr<SceneModuleManagerInterface>;
 using DebugPublisher = tier4_autoware_utils::DebugPublisher;
 using DebugDoubleMsg = tier4_debug_msgs::msg::Float64Stamped;
+using DebugStringMsg = tier4_debug_msgs::msg::StringStamped;
 
 enum Action {
   ADD = 0,
@@ -447,6 +448,8 @@ class PlannerManager
 
   std::unique_ptr<DebugPublisher> debug_publisher_ptr_;
 
+  std::unique_ptr<DebugPublisher> state_publisher_ptr_;
+
   pluginlib::ClassLoader<SceneModuleManagerInterface> plugin_loader_;
 
   mutable rclcpp::Logger logger_;

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<DebugPublisher>(&node, "~/debug");
+  state_publisher_ptr_ = std::make_unique<DebugPublisher>(&node, "~/debug");
 }
 
 void PlannerManager::launchScenePlugin(rclcpp::Node & node, const std::string & name)
@@ -889,10 +890,6 @@ void PlannerManager::resetRootLanelet(const std::shared_ptr<PlannerData> & data)
 
 void PlannerManager::print() const
 {
-  if (!verbose_) {
-    return;
-  }
-
   const auto get_status = [](const auto & m) {
     return magic_enum::enum_name(m->getCurrentStatus());
   };
@@ -941,6 +938,12 @@ void PlannerManager::print() const
                   << std::setw(21);
   }
 
+  state_publisher_ptr_->publish<DebugStringMsg>("internal_state", string_stream.str());
+
+  if (!verbose_) {
+    return;
+  }
+
   RCLCPP_INFO_STREAM(logger_, string_stream.str());
 }