@@ -17,17 +17,16 @@ namespace autoware_minimap_overlay_rviz_plugin
17
17
VehicleMapDisplay::VehicleMapDisplay () : rviz_common::Display(), overlay_(nullptr )
18
18
{
19
19
property_width_ = new rviz_common::properties::IntProperty (
20
- " Width" , 300 , " Width of the overlay" , this , SLOT (updateOverlaySize ()));
20
+ " Width" , 256 , " Width of the overlay" , this , SLOT (updateOverlaySize ()));
21
21
property_height_ = new rviz_common::properties::IntProperty (
22
- " Height" , 300 , " Height of the overlay" , this , SLOT (updateOverlaySize ()));
22
+ " Height" , 256 , " Height of the overlay" , this , SLOT (updateOverlaySize ()));
23
23
property_left_ = new rviz_common::properties::IntProperty (
24
- " Left" , 0 , " Left position of the overlay" , this , SLOT (updateOverlayPosition ()));
24
+ " Left" , 15 , " Left position of the overlay" , this , SLOT (updateOverlayPosition ()));
25
25
property_top_ = new rviz_common::properties::IntProperty (
26
- " Top" , 0 , " Top position of the overlay" , this , SLOT (updateOverlayPosition ()));
26
+ " Top" , 15 , " Top position of the overlay" , this , SLOT (updateOverlayPosition ()));
27
27
28
28
alpha_property_ = new rviz_common::properties::FloatProperty (
29
- " Alpha" , 0.7 , " Amount of transparency to apply to the overlay." , this ,
30
- SLOT (updateOverlaySize ()));
29
+ " Alpha" , 0 , " Amount of transparency to apply to the overlay." , this , SLOT (updateOverlaySize ()));
31
30
32
31
background_color_property_ = new rviz_common::properties::ColorProperty (
33
32
" Background Color" , QColor (0 , 0 , 0 ), " Color to draw the background." , this ,
@@ -39,8 +38,20 @@ VehicleMapDisplay::VehicleMapDisplay() : rviz_common::Display(), overlay_(nullpt
39
38
property_zoom_->setMin (15 );
40
39
property_zoom_->setMax (19 );
41
40
41
+ property_latitude_ = new rviz_common::properties::FloatProperty (
42
+ " Latitude" , 0.0 , " Latitude of the center position" , this , SLOT (updateLatitude ()));
43
+
44
+ property_longitude_ = new rviz_common::properties::FloatProperty (
45
+ " Longitude" , 0.0 , " Longitude of the center position" , this , SLOT (updateLongitude ()));
46
+
47
+ property_topic_ = new rviz_common::properties::StringProperty (
48
+ " Topic" , " fix" , " NavSatFix topic to subscribe to" , this , SLOT (updateTopic ()));
49
+
42
50
zoom_ = property_zoom_->getInt ();
43
51
52
+ latitude_ = property_latitude_->getFloat ();
53
+ longitude_ = property_longitude_->getFloat ();
54
+
44
55
tile_field_ = std::make_unique<TileField>(this );
45
56
connect (tile_field_.get (), &TileField::tilesUpdated, this , &VehicleMapDisplay::onTilesUpdated);
46
57
}
@@ -61,8 +72,7 @@ void VehicleMapDisplay::onInitialize()
61
72
updateOverlayPosition ();
62
73
63
74
node_ = context_->getRosNodeAbstraction ().lock ()->get_raw_node ();
64
- nav_sat_fix_sub_ = node_->create_subscription <sensor_msgs::msg::NavSatFix>(
65
- " fix" , 10 , std::bind (&VehicleMapDisplay::navSatFixCallback, this , std::placeholders::_1));
75
+ updateTopic ();
66
76
}
67
77
68
78
void VehicleMapDisplay::reset ()
@@ -150,7 +160,7 @@ void VehicleMapDisplay::drawCircle(QPainter & painter, const QRectF & background
150
160
painter.setBrush (colorFromHSV);
151
161
152
162
// Define the visible rectangle
153
- QRectF visibleRect (backgroundRect.width () / 2 - 150 , backgroundRect.height () / 2 - 150 , 300 , 300 );
163
+ QRectF visibleRect (backgroundRect.width () / 2 - 112 , backgroundRect.height () / 2 - 112 , 225 , 225 );
154
164
155
165
// Define the circular clipping path
156
166
QPainterPath path;
@@ -201,20 +211,52 @@ void VehicleMapDisplay::onTilesUpdated()
201
211
void VehicleMapDisplay::updateZoomLevel ()
202
212
{
203
213
zoom_ = property_zoom_->getInt (); // Update the zoom level
204
- queueRender (); // Request re-rendering
214
+ tile_field_->fetchTiles (zoom_, center_x_tile_, center_y_tile_);
215
+ queueRender (); // Request re-rendering
216
+ }
217
+
218
+ void VehicleMapDisplay::updateLatitude ()
219
+ {
220
+ latitude_ = property_latitude_->getFloat ();
221
+ int new_center_x_tile = tile_field_->long2tilex (longitude_, zoom_);
222
+ int new_center_y_tile = tile_field_->lat2tiley (latitude_, zoom_);
223
+ center_x_tile_ = new_center_x_tile;
224
+ center_y_tile_ = new_center_y_tile;
225
+ tile_field_->fetchTiles (zoom_, center_x_tile_, center_y_tile_);
226
+ queueRender ();
227
+ }
228
+
229
+ void VehicleMapDisplay::updateLongitude ()
230
+ {
231
+ longitude_ = property_longitude_->getFloat ();
232
+ int new_center_x_tile = tile_field_->long2tilex (longitude_, zoom_);
233
+ int new_center_y_tile = tile_field_->lat2tiley (latitude_, zoom_);
234
+ center_x_tile_ = new_center_x_tile;
235
+ center_y_tile_ = new_center_y_tile;
236
+ tile_field_->fetchTiles (zoom_, center_x_tile_, center_y_tile_);
237
+ queueRender ();
238
+ }
239
+
240
+ void VehicleMapDisplay::updateTopic ()
241
+ {
242
+ std::string topic = property_topic_->getStdString ();
243
+ if (!topic.empty ()) {
244
+ nav_sat_fix_sub_ = node_->create_subscription <sensor_msgs::msg::NavSatFix>(
245
+ topic, 10 , std::bind (&VehicleMapDisplay::navSatFixCallback, this , std::placeholders::_1));
246
+ }
205
247
}
206
248
207
249
void VehicleMapDisplay::navSatFixCallback (const sensor_msgs::msg::NavSatFix::SharedPtr msg)
208
250
{
209
251
// Convert GPS coordinates to tile coordinates
210
- double lat = msg->latitude ;
211
- double lon = msg->longitude ;
252
+ latitude_ = msg->latitude ;
253
+ longitude_ = msg->longitude ;
212
254
213
- latitude_ = lat ;
214
- longitude_ = lon ;
255
+ property_longitude_-> setFloat (longitude_) ;
256
+ property_latitude_-> setFloat (latitude_) ;
215
257
216
- int new_center_x_tile = tile_field_->long2tilex (lon , zoom_);
217
- int new_center_y_tile = tile_field_->lat2tiley (lat , zoom_);
258
+ int new_center_x_tile = tile_field_->long2tilex (longitude_ , zoom_);
259
+ int new_center_y_tile = tile_field_->lat2tiley (latitude_ , zoom_);
218
260
219
261
center_x_tile_ = new_center_x_tile;
220
262
center_y_tile_ = new_center_y_tile;
0 commit comments