Skip to content

Commit 82f470f

Browse files
author
KhalilSelyan
committed
feat: Update VehicleMapDisplay to include latitude, longitude, and topic properties
Signed-off-by: KhalilSelyan <khalil@leodrive.ai>
1 parent ac08766 commit 82f470f

File tree

2 files changed

+64
-16
lines changed

2 files changed

+64
-16
lines changed

common/autoware_overlay_rviz_plugin/autoware_minimap_overlay_rviz_plugin/src/include/minimap.hpp

+6
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,9 @@ protected Q_SLOTS:
5353
void updateOverlayPosition();
5454
void onTilesUpdated();
5555
void updateZoomLevel();
56+
void updateLatitude();
57+
void updateLongitude();
58+
void updateTopic();
5659

5760
protected:
5861
void onEnable() override;
@@ -76,6 +79,9 @@ protected Q_SLOTS:
7679
rviz_common::properties::IntProperty * property_zoom_;
7780
rviz_common::properties::FloatProperty * alpha_property_;
7881
rviz_common::properties::ColorProperty * background_color_property_;
82+
rviz_common::properties::FloatProperty * property_latitude_;
83+
rviz_common::properties::FloatProperty * property_longitude_;
84+
rviz_common::properties::StringProperty * property_topic_;
7985

8086
int zoom_;
8187

common/autoware_overlay_rviz_plugin/autoware_minimap_overlay_rviz_plugin/src/minimap.cpp

+58-16
Original file line numberDiff line numberDiff line change
@@ -17,17 +17,16 @@ namespace autoware_minimap_overlay_rviz_plugin
1717
VehicleMapDisplay::VehicleMapDisplay() : rviz_common::Display(), overlay_(nullptr)
1818
{
1919
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()));
2121
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()));
2323
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()));
2525
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()));
2727

2828
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()));
3130

3231
background_color_property_ = new rviz_common::properties::ColorProperty(
3332
"Background Color", QColor(0, 0, 0), "Color to draw the background.", this,
@@ -39,8 +38,20 @@ VehicleMapDisplay::VehicleMapDisplay() : rviz_common::Display(), overlay_(nullpt
3938
property_zoom_->setMin(15);
4039
property_zoom_->setMax(19);
4140

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+
4250
zoom_ = property_zoom_->getInt();
4351

52+
latitude_ = property_latitude_->getFloat();
53+
longitude_ = property_longitude_->getFloat();
54+
4455
tile_field_ = std::make_unique<TileField>(this);
4556
connect(tile_field_.get(), &TileField::tilesUpdated, this, &VehicleMapDisplay::onTilesUpdated);
4657
}
@@ -61,8 +72,7 @@ void VehicleMapDisplay::onInitialize()
6172
updateOverlayPosition();
6273

6374
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();
6676
}
6777

6878
void VehicleMapDisplay::reset()
@@ -150,7 +160,7 @@ void VehicleMapDisplay::drawCircle(QPainter & painter, const QRectF & background
150160
painter.setBrush(colorFromHSV);
151161

152162
// 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);
154164

155165
// Define the circular clipping path
156166
QPainterPath path;
@@ -201,20 +211,52 @@ void VehicleMapDisplay::onTilesUpdated()
201211
void VehicleMapDisplay::updateZoomLevel()
202212
{
203213
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+
}
205247
}
206248

207249
void VehicleMapDisplay::navSatFixCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg)
208250
{
209251
// Convert GPS coordinates to tile coordinates
210-
double lat = msg->latitude;
211-
double lon = msg->longitude;
252+
latitude_ = msg->latitude;
253+
longitude_ = msg->longitude;
212254

213-
latitude_ = lat;
214-
longitude_ = lon;
255+
property_longitude_->setFloat(longitude_);
256+
property_latitude_->setFloat(latitude_);
215257

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_);
218260

219261
center_x_tile_ = new_center_x_tile;
220262
center_y_tile_ = new_center_y_tile;

0 commit comments

Comments
 (0)