16
16
17
17
#include < QFontDatabase>
18
18
#include < QPainter>
19
+ #include < memory>
19
20
#include < rclcpp/rclcpp.hpp>
20
21
#include < rviz_common/properties/ros_topic_property.hpp>
21
22
#include < rviz_rendering/render_system.hpp>
@@ -32,13 +33,13 @@ namespace autoware::mission_details_overlay_rviz_plugin
32
33
MissionDetailsDisplay::MissionDetailsDisplay ()
33
34
{
34
35
property_width_ = new rviz_common::properties::IntProperty (
35
- " Width" , 170 , " Width of the overlay" , this , SLOT (updateOverlaySize ()));
36
+ " Width" , 170 , " Width of the overlay" , this , SLOT (update_size ()));
36
37
property_height_ = new rviz_common::properties::IntProperty (
37
- " Height" , 100 , " Height of the overlay" , this , SLOT (updateOverlaySize ()));
38
+ " Height" , 100 , " Height of the overlay" , this , SLOT (update_size ()));
38
39
property_right_ = new rviz_common::properties::IntProperty (
39
- " Right" , 10 , " Margin from the right border" , this , SLOT (updateOverlaySize ()));
40
+ " Right" , 10 , " Margin from the right border" , this , SLOT (update_size ()));
40
41
property_top_ = new rviz_common::properties::IntProperty (
41
- " Top" , 10 , " Margin from the top border" , this , SLOT (updateOverlaySize ()));
42
+ " Top" , 10 , " Margin from the top border" , this , SLOT (update_size ()));
42
43
43
44
// Initialize the component displays
44
45
remaining_distance_time_display_ = std::make_unique<RemainingDistanceTimeDisplay>();
@@ -53,9 +54,9 @@ void MissionDetailsDisplay::onInitialize()
53
54
static int count = 0 ;
54
55
std::stringstream ss;
55
56
ss << " MissionDetailsDisplay" << count++;
56
- overlay_. reset ( new autoware::mission_details_overlay_rviz_plugin::OverlayObject (ss.str () ));
57
+ overlay_ = std::make_shared< autoware::mission_details_overlay_rviz_plugin::OverlayObject> (ss.str ());
57
58
overlay_->show ();
58
- updateOverlaySize ();
59
+ update_size ();
59
60
60
61
auto rviz_ros_node = context_->getRosNodeAbstraction ();
61
62
@@ -78,7 +79,7 @@ void MissionDetailsDisplay::setupRosSubscriptions()
78
79
remaining_distance_time_topic_property_->getTopicStd (),
79
80
rclcpp::QoS (rclcpp::KeepLast (10 )).durability_volatile ().reliable (),
80
81
[this ](const autoware_internal_msgs::msg::MissionRemainingDistanceTime::SharedPtr msg) {
81
- updateRemainingDistanceTimeData (msg);
82
+ cb_remaining_distance_time (msg);
82
83
});
83
84
}
84
85
@@ -104,7 +105,7 @@ void MissionDetailsDisplay::update(float wall_dt, float ros_dt)
104
105
autoware::mission_details_overlay_rviz_plugin::ScopedPixelBuffer buffer = overlay_->getBuffer ();
105
106
QImage hud = buffer.getQImage (*overlay_);
106
107
hud.fill (Qt::transparent);
107
- drawWidget (hud);
108
+ draw_widget (hud);
108
109
}
109
110
110
111
void MissionDetailsDisplay::onEnable ()
@@ -127,7 +128,7 @@ void MissionDetailsDisplay::onDisable()
127
128
}
128
129
}
129
130
130
- void MissionDetailsDisplay::updateRemainingDistanceTimeData (
131
+ void MissionDetailsDisplay::cb_remaining_distance_time (
131
132
const autoware_internal_msgs::msg::MissionRemainingDistanceTime::ConstSharedPtr & msg)
132
133
{
133
134
std::lock_guard<std::mutex> lock (property_mutex_);
@@ -138,7 +139,7 @@ void MissionDetailsDisplay::updateRemainingDistanceTimeData(
138
139
}
139
140
}
140
141
141
- void MissionDetailsDisplay::drawWidget (QImage & hud)
142
+ void MissionDetailsDisplay::draw_widget (QImage & hud)
142
143
{
143
144
std::lock_guard<std::mutex> lock (property_mutex_);
144
145
@@ -150,7 +151,7 @@ void MissionDetailsDisplay::drawWidget(QImage & hud)
150
151
painter.setRenderHint (QPainter::Antialiasing, true );
151
152
152
153
QRectF backgroundRect (0 , 0 , qreal (property_width_->getInt ()), qreal (property_height_->getInt ()));
153
- drawHorizontalRoundedRectangle (painter, backgroundRect);
154
+ draw_rounded_rect (painter, backgroundRect);
154
155
155
156
if (remaining_distance_time_display_) {
156
157
remaining_distance_time_display_->drawRemainingDistanceTimeDisplay (painter, backgroundRect);
@@ -159,33 +160,18 @@ void MissionDetailsDisplay::drawWidget(QImage & hud)
159
160
painter.end ();
160
161
}
161
162
162
- void MissionDetailsDisplay::drawHorizontalRoundedRectangle (
163
+ void MissionDetailsDisplay::draw_rounded_rect (
163
164
QPainter & painter, const QRectF & backgroundRect)
164
165
{
165
166
painter.setRenderHint (QPainter::Antialiasing, true );
166
167
QColor colorFromHSV;
167
- colorFromHSV.setHsv (0 , 0 , 29 ); // Hue, Saturation, Value
168
- colorFromHSV.setAlphaF (0.60 ); // Transparency
168
+ colorFromHSV.setHsv (0 , 0 , 29 );
169
+ colorFromHSV.setAlphaF (0.60 );
169
170
170
171
painter.setBrush (colorFromHSV);
171
172
172
173
painter.setPen (Qt::NoPen);
173
- painter.drawRoundedRect (
174
- backgroundRect, backgroundRect.height () / 2 , backgroundRect.height () / 2 ); // Circular ends
175
- }
176
- void MissionDetailsDisplay::drawVerticalRoundedRectangle (
177
- QPainter & painter, const QRectF & backgroundRect)
178
- {
179
- painter.setRenderHint (QPainter::Antialiasing, true );
180
- QColor colorFromHSV;
181
- colorFromHSV.setHsv (0 , 0 , 0 ); // Hue, Saturation, Value
182
- colorFromHSV.setAlphaF (0.65 ); // Transparency
183
-
184
- painter.setBrush (colorFromHSV);
185
-
186
- painter.setPen (Qt::NoPen);
187
- painter.drawRoundedRect (
188
- backgroundRect, backgroundRect.width () / 2 , backgroundRect.width () / 2 ); // Circular ends
174
+ painter.drawRoundedRect (backgroundRect, backgroundRect.height () / 2 , backgroundRect.height () / 2 );
189
175
}
190
176
191
177
void MissionDetailsDisplay::reset ()
@@ -194,7 +180,7 @@ void MissionDetailsDisplay::reset()
194
180
overlay_->hide ();
195
181
}
196
182
197
- void MissionDetailsDisplay::updateOverlaySize ()
183
+ void MissionDetailsDisplay::update_size ()
198
184
{
199
185
std::lock_guard<std::mutex> lock (mutex_);
200
186
overlay_->updateTextureSize (property_width_->getInt (), property_height_->getInt ());
@@ -205,12 +191,6 @@ void MissionDetailsDisplay::updateOverlaySize()
205
191
queueRender ();
206
192
}
207
193
208
- void MissionDetailsDisplay::updateOverlayColor ()
209
- {
210
- std::lock_guard<std::mutex> lock (mutex_);
211
- queueRender ();
212
- }
213
-
214
194
void MissionDetailsDisplay::topic_updated_remaining_distance_time ()
215
195
{
216
196
// resubscribe to the topic
@@ -222,7 +202,7 @@ void MissionDetailsDisplay::topic_updated_remaining_distance_time()
222
202
remaining_distance_time_topic_property_->getTopicStd (),
223
203
rclcpp::QoS (rclcpp::KeepLast (10 )).durability_volatile ().reliable (),
224
204
[this ](const autoware_internal_msgs::msg::MissionRemainingDistanceTime::SharedPtr msg) {
225
- updateRemainingDistanceTimeData (msg);
205
+ cb_remaining_distance_time (msg);
226
206
});
227
207
}
228
208
0 commit comments