Skip to content

Commit ed926b3

Browse files
takayuki5168takayuki5168
authored
Merge pull request #1593 from tier4/hotfix/v3.0.2/output-internal-info
fix: cherry pick PRs to fix invalid access
2 parents 1a9d2d8 + 99d0b71 commit ed926b3

File tree

8 files changed

+328
-6
lines changed

8 files changed

+328
-6
lines changed

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

+6-2
Original file line numberDiff line numberDiff line change
@@ -590,8 +590,12 @@ double calcLateralOffset(
590590
return std::nan("");
591591
}
592592

593-
const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx));
594-
const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx + 1));
593+
const auto p_indices = overlap_removed_points.size() - 2;
594+
const auto p_front_idx = (p_indices > seg_idx) ? seg_idx : p_indices;
595+
const auto p_back_idx = p_front_idx + 1;
596+
597+
const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(p_front_idx));
598+
const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(p_back_idx));
595599

596600
const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0};
597601
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

+2
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,9 @@ add_definitions(-DQT_NO_KEYWORDS)
1212
ament_auto_add_library(tier4_debug_rviz_plugin SHARED
1313
include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp
1414
include/tier4_debug_rviz_plugin/jsk_overlay_utils.hpp
15+
include/tier4_debug_rviz_plugin/string_stamped.hpp
1516
src/float32_multi_array_stamped_pie_chart.cpp
17+
src/string_stamped.cpp
1618
src/jsk_overlay_utils.cpp
1719
)
1820

‎common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png

18.4 KB
Loading

‎common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp

+107
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,107 @@
1+
// Copyright 2023 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
// Copyright (c) 2014, JSK Lab
16+
// All rights reserved.
17+
//
18+
// Software License Agreement (BSD License)
19+
//
20+
// Redistribution and use in source and binary forms, with or without
21+
// modification, are permitted provided that the following conditions
22+
// are met:
23+
//
24+
// * Redistributions of source code must retain the above copyright
25+
// notice, this list of conditions and the following disclaimer.
26+
// * Redistributions in binary form must reproduce the above
27+
// copyright notice, this list of conditions and the following
28+
// disclaimer in the documentation and/or other materials provided
29+
// with the distribution.
30+
// * Neither the name of {copyright_holder} nor the names of its
31+
// contributors may be used to endorse or promote products derived
32+
// from this software without specific prior written permission.
33+
//
34+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
35+
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
36+
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
37+
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
38+
// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
39+
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
40+
// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
41+
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
42+
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
43+
// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
44+
// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
45+
// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE
46+
// POSSIBILITY OF SUCH DAMAGE.
47+
48+
#ifndef TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_
49+
#define TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_
50+
51+
#include <memory>
52+
#include <mutex>
53+
54+
#ifndef Q_MOC_RUN
55+
#include "tier4_debug_rviz_plugin/jsk_overlay_utils.hpp"
56+
57+
#include <rviz_common/properties/color_property.hpp>
58+
#include <rviz_common/properties/float_property.hpp>
59+
#include <rviz_common/properties/int_property.hpp>
60+
#include <rviz_common/ros_topic_display.hpp>
61+
62+
#endif
63+
64+
#include <tier4_debug_msgs/msg/string_stamped.hpp>
65+
66+
namespace rviz_plugins
67+
{
68+
class StringStampedOverlayDisplay
69+
: public rviz_common::RosTopicDisplay<tier4_debug_msgs::msg::StringStamped>
70+
71+
{
72+
Q_OBJECT
73+
74+
public:
75+
StringStampedOverlayDisplay();
76+
~StringStampedOverlayDisplay() override;
77+
78+
void onInitialize() override;
79+
void onEnable() override;
80+
void onDisable() override;
81+
82+
private Q_SLOTS:
83+
void updateVisualization();
84+
85+
protected:
86+
void update(float wall_dt, float ros_dt) override;
87+
void processMessage(const tier4_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) override;
88+
jsk_rviz_plugins::OverlayObject::Ptr overlay_;
89+
rviz_common::properties::ColorProperty * property_text_color_;
90+
rviz_common::properties::IntProperty * property_left_;
91+
rviz_common::properties::IntProperty * property_top_;
92+
rviz_common::properties::IntProperty * property_value_height_offset_;
93+
rviz_common::properties::FloatProperty * property_value_scale_;
94+
rviz_common::properties::IntProperty * property_font_size_;
95+
rviz_common::properties::IntProperty * property_max_letter_num_;
96+
// QImage hud_;
97+
98+
private:
99+
static constexpr int line_width_ = 2;
100+
static constexpr int hand_width_ = 4;
101+
102+
std::mutex mutex_;
103+
tier4_debug_msgs::msg::StringStamped::ConstSharedPtr last_msg_ptr_;
104+
};
105+
} // namespace rviz_plugins
106+
107+
#endif // TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_

‎common/tier4_debug_rviz_plugin/plugins/plugin_description.xml

+5
Original file line numberDiff line numberDiff line change
@@ -4,4 +4,9 @@
44
base_class_type="rviz_common::Display">
55
<description>Display drivable area of tier4_debug_msgs::msg::Float32MultiArrayStamped</description>
66
</class>
7+
<class name="rviz_plugins/StringStampedOverlayDisplay"
8+
type="rviz_plugins::StringStampedOverlayDisplay"
9+
base_class_type="rviz_common::Display">
10+
<description>Display drivable area of tier4_debug_msgs::msg::StringStamped</description>
11+
</class>
712
</library>

‎common/tier4_debug_rviz_plugin/src/string_stamped.cpp

+198
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,198 @@
1+
// Copyright 2023 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
// Copyright (c) 2014, JSK Lab
16+
// All rights reserved.
17+
//
18+
// Software License Agreement (BSD License)
19+
//
20+
// Redistribution and use in source and binary forms, with or without
21+
// modification, are permitted provided that the following conditions
22+
// are met:
23+
//
24+
// * Redistributions of source code must retain the above copyright
25+
// notice, this list of conditions and the following disclaimer.
26+
// * Redistributions in binary form must reproduce the above
27+
// copyright notice, this list of conditions and the following
28+
// disclaimer in the documentation and/or other materials provided
29+
// with the distribution.
30+
// * Neither the name of {copyright_holder} nor the names of its
31+
// contributors may be used to endorse or promote products derived
32+
// from this software without specific prior written permission.
33+
//
34+
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
35+
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
36+
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
37+
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
38+
// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
39+
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
40+
// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
41+
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
42+
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
43+
// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
44+
// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
45+
// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE
46+
// POSSIBILITY OF SUCH DAMAGE.
47+
48+
#include "tier4_debug_rviz_plugin/string_stamped.hpp"
49+
50+
#include "tier4_debug_rviz_plugin/jsk_overlay_utils.hpp"
51+
52+
#include <QPainter>
53+
#include <rviz_common/uniform_string_stream.hpp>
54+
55+
#include <X11/Xlib.h>
56+
57+
#include <algorithm>
58+
#include <optional>
59+
#include <string>
60+
#include <vector>
61+
62+
namespace rviz_plugins
63+
{
64+
StringStampedOverlayDisplay::StringStampedOverlayDisplay()
65+
{
66+
const Screen * screen_info = DefaultScreenOfDisplay(XOpenDisplay(NULL));
67+
68+
constexpr float hight_4k = 2160.0;
69+
const float scale = static_cast<float>(screen_info->height) / hight_4k;
70+
const auto left = static_cast<int>(std::round(1024 * scale));
71+
const auto top = static_cast<int>(std::round(128 * scale));
72+
73+
property_text_color_ = new rviz_common::properties::ColorProperty(
74+
"Text Color", QColor(25, 255, 240), "text color", this, SLOT(updateVisualization()), this);
75+
property_left_ = new rviz_common::properties::IntProperty(
76+
"Left", left, "Left of the plotter window", this, SLOT(updateVisualization()), this);
77+
property_left_->setMin(0);
78+
property_top_ = new rviz_common::properties::IntProperty(
79+
"Top", top, "Top of the plotter window", this, SLOT(updateVisualization()));
80+
property_top_->setMin(0);
81+
82+
property_value_height_offset_ = new rviz_common::properties::IntProperty(
83+
"Value height offset", 0, "Height offset of the plotter window", this,
84+
SLOT(updateVisualization()));
85+
property_font_size_ = new rviz_common::properties::IntProperty(
86+
"Font Size", 15, "Font Size", this, SLOT(updateVisualization()), this);
87+
property_font_size_->setMin(1);
88+
property_max_letter_num_ = new rviz_common::properties::IntProperty(
89+
"Max Letter Num", 100, "Max Letter Num", this, SLOT(updateVisualization()), this);
90+
property_max_letter_num_->setMin(10);
91+
}
92+
93+
StringStampedOverlayDisplay::~StringStampedOverlayDisplay()
94+
{
95+
if (initialized()) {
96+
overlay_->hide();
97+
}
98+
}
99+
100+
void StringStampedOverlayDisplay::onInitialize()
101+
{
102+
RTDClass::onInitialize();
103+
104+
static int count = 0;
105+
rviz_common::UniformStringStream ss;
106+
ss << "StringOverlayDisplayObject" << count++;
107+
auto logger = context_->getRosNodeAbstraction().lock()->get_raw_node()->get_logger();
108+
overlay_.reset(new jsk_rviz_plugins::OverlayObject(scene_manager_, logger, ss.str()));
109+
110+
overlay_->show();
111+
112+
const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt();
113+
overlay_->updateTextureSize(texture_size, texture_size);
114+
overlay_->setPosition(property_left_->getInt(), property_top_->getInt());
115+
overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight());
116+
}
117+
118+
void StringStampedOverlayDisplay::onEnable()
119+
{
120+
subscribe();
121+
overlay_->show();
122+
}
123+
124+
void StringStampedOverlayDisplay::onDisable()
125+
{
126+
unsubscribe();
127+
reset();
128+
overlay_->hide();
129+
}
130+
131+
void StringStampedOverlayDisplay::update(float wall_dt, float ros_dt)
132+
{
133+
(void)wall_dt;
134+
(void)ros_dt;
135+
136+
std::lock_guard<std::mutex> message_lock(mutex_);
137+
if (!last_msg_ptr_) {
138+
return;
139+
}
140+
141+
// Display
142+
QColor background_color;
143+
background_color.setAlpha(0);
144+
jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer();
145+
QImage hud = buffer.getQImage(*overlay_);
146+
hud.fill(background_color);
147+
148+
QPainter painter(&hud);
149+
painter.setRenderHint(QPainter::Antialiasing, true);
150+
151+
const int w = overlay_->getTextureWidth() - line_width_;
152+
const int h = overlay_->getTextureHeight() - line_width_;
153+
154+
// text
155+
QColor text_color(property_text_color_->getColor());
156+
text_color.setAlpha(255);
157+
painter.setPen(QPen(text_color, static_cast<int>(2), Qt::SolidLine));
158+
QFont font = painter.font();
159+
font.setPixelSize(property_font_size_->getInt());
160+
font.setBold(true);
161+
painter.setFont(font);
162+
163+
// same as above, but align on right side
164+
painter.drawText(
165+
0, std::min(property_value_height_offset_->getInt(), h - 1), w,
166+
std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignLeft | Qt::AlignTop,
167+
last_msg_ptr_->data.c_str());
168+
painter.end();
169+
updateVisualization();
170+
}
171+
172+
void StringStampedOverlayDisplay::processMessage(
173+
const tier4_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr)
174+
{
175+
if (!isEnabled()) {
176+
return;
177+
}
178+
179+
{
180+
std::lock_guard<std::mutex> message_lock(mutex_);
181+
last_msg_ptr_ = msg_ptr;
182+
}
183+
184+
queueRender();
185+
}
186+
187+
void StringStampedOverlayDisplay::updateVisualization()
188+
{
189+
const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt();
190+
overlay_->updateTextureSize(texture_size, texture_size);
191+
overlay_->setPosition(property_left_->getInt(), property_top_->getInt());
192+
overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight());
193+
}
194+
195+
} // namespace rviz_plugins
196+
197+
#include <pluginlib/class_list_macros.hpp>
198+
PLUGINLIB_EXPORT_CLASS(rviz_plugins::StringStampedOverlayDisplay, rviz_common::Display)

‎planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

+3
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,7 @@ using SceneModulePtr = std::shared_ptr<SceneModuleInterface>;
4646
using SceneModuleManagerPtr = std::shared_ptr<SceneModuleManagerInterface>;
4747
using DebugPublisher = tier4_autoware_utils::DebugPublisher;
4848
using DebugDoubleMsg = tier4_debug_msgs::msg::Float64Stamped;
49+
using DebugStringMsg = tier4_debug_msgs::msg::StringStamped;
4950

5051
enum Action {
5152
ADD = 0,
@@ -447,6 +448,8 @@ class PlannerManager
447448

448449
std::unique_ptr<DebugPublisher> debug_publisher_ptr_;
449450

451+
std::unique_ptr<DebugPublisher> state_publisher_ptr_;
452+
450453
pluginlib::ClassLoader<SceneModuleManagerInterface> plugin_loader_;
451454

452455
mutable rclcpp::Logger logger_;

‎planning/behavior_path_planner/src/planner_manager.cpp

+7-4
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,7 @@ PlannerManager::PlannerManager(
4040
{
4141
processing_time_.emplace("total_time", 0.0);
4242
debug_publisher_ptr_ = std::make_unique<DebugPublisher>(&node, "~/debug");
43+
state_publisher_ptr_ = std::make_unique<DebugPublisher>(&node, "~/debug");
4344
}
4445

4546
void PlannerManager::launchScenePlugin(rclcpp::Node & node, const std::string & name)
@@ -889,10 +890,6 @@ void PlannerManager::resetRootLanelet(const std::shared_ptr<PlannerData> & data)
889890

890891
void PlannerManager::print() const
891892
{
892-
if (!verbose_) {
893-
return;
894-
}
895-
896893
const auto get_status = [](const auto & m) {
897894
return magic_enum::enum_name(m->getCurrentStatus());
898895
};
@@ -941,6 +938,12 @@ void PlannerManager::print() const
941938
<< std::setw(21);
942939
}
943940

941+
state_publisher_ptr_->publish<DebugStringMsg>("internal_state", string_stream.str());
942+
943+
if (!verbose_) {
944+
return;
945+
}
946+
944947
RCLCPP_INFO_STREAM(logger_, string_stream.str());
945948
}
946949

0 commit comments

Comments
 (0)