Skip to content

Commit 787288e

Browse files
Jaeyoung-LimJaeyoung-Lim
committed
Add terrain loader node
Use gdal transforms Remove geoconversion library F F
1 parent 5f0da73 commit 787288e

File tree

6 files changed

+335
-12
lines changed

6 files changed

+335
-12
lines changed

‎grid_map_geo/CMakeLists.txt‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,13 @@ add_executable(test_tif_loader
3434
add_dependencies(test_tif_loader ${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${GDAL_LIBRARY})
3535
target_link_libraries(test_tif_loader ${PROJECT_NAME} ${catkin_LIBRARIES} ${GDAL_LIBRARY} ${OpenCV_LIBRARIES})
3636

37+
add_executable(terrain_loader
38+
src/terrain_loader.cpp
39+
)
40+
add_dependencies(terrain_loader ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${GDAL_LIBRARY})
41+
target_link_libraries(terrain_loader ${PROJECT_NAME} ${catkin_LIBRARIES} ${GDAL_LIBRARY} ${OpenCV_LIBRARIES})
42+
43+
3744
if(CATKIN_ENABLE_TESTING)
3845
# Add gtest based cpp test target and link libraries
3946
catkin_add_gtest(${PROJECT_NAME}-test test/main.cpp

‎grid_map_geo/include/grid_map_geo/grid_map_geo.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -107,7 +107,7 @@ class GridMapGeo {
107107
*/
108108
bool initializeFromGeotiff(const std::string& path);
109109

110-
bool initializeFromVrt(const std::string& path, const Eigen::Vector2d &map_center, Eigen::Vector2d &extent);
110+
bool initializeFromVrt(const std::string& path, const Eigen::Vector2d& map_center, Eigen::Vector2d& extent);
111111

112112
/**
113113
* @brief Load a color layer from a geotiff file (orthomosaic)

‎grid_map_geo/launch/run_terrain_loader.launch‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
<launch>
2+
<arg name="visualization" default="true"/>
3+
<arg name="location" default="hinwil"/>
4+
5+
<node pkg="grid_map_geo" type="terrain_loader" name="terrain_loader" output="screen">
6+
<!-- <param name="terrain_path" value="$(find terrain_models)/models/$(arg location).tif"/> -->
7+
<param name="terrain_path" value="/vsizip/vsicurl/https://terrain.ardupilot.org/SRTM1/ap_srtm1.zip/ap_srtm1.vrt"/>
8+
</node>
9+
10+
<group if="$(arg visualization)">
11+
<node type="rviz" name="rviz" pkg="rviz" args="-d $(find grid_map_geo)/launch/terrain_loader.rviz" output="screen"/>
12+
</group>
13+
</launch>

‎grid_map_geo/launch/terrain_loader.rviz‎

Lines changed: 154 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,154 @@
1+
Panels:
2+
- Class: rviz/Displays
3+
Help Height: 78
4+
Name: Displays
5+
Property Tree Widget:
6+
Expanded:
7+
- /Global Options1
8+
- /Grid1
9+
- /GridMap1
10+
Splitter Ratio: 0.5
11+
Tree Height: 850
12+
- Class: rviz/Selection
13+
Name: Selection
14+
- Class: rviz/Tool Properties
15+
Expanded:
16+
- /2D Pose Estimate1
17+
- /2D Nav Goal1
18+
- /Publish Point1
19+
Name: Tool Properties
20+
Splitter Ratio: 0.5886790156364441
21+
- Class: rviz/Views
22+
Expanded:
23+
- /Current View1
24+
Name: Views
25+
Splitter Ratio: 0.5
26+
- Class: rviz/Time
27+
Name: Time
28+
SyncMode: 0
29+
SyncSource: ""
30+
- Class: rviz_plugin_tutorials/Teleop
31+
Name: Teleop
32+
Topic: ""
33+
- Class: mav_planning_rviz/PlanningPanel
34+
Name: PlanningPanel
35+
namespace: ""
36+
odometry_topic: ""
37+
planner_name: sertig
38+
planning_budget: 4
39+
Preferences:
40+
PromptSaveOnExit: true
41+
Toolbars:
42+
toolButtonStyle: 2
43+
Visualization Manager:
44+
Class: ""
45+
Displays:
46+
- Alpha: 0.5
47+
Cell Size: 1000
48+
Class: rviz/Grid
49+
Color: 160; 160; 164
50+
Enabled: true
51+
Line Style:
52+
Line Width: 0.029999999329447746
53+
Value: Lines
54+
Name: Grid
55+
Normal Cell Count: 0
56+
Offset:
57+
X: 0
58+
Y: 0
59+
Z: 0
60+
Plane: XY
61+
Plane Cell Count: 10
62+
Reference Frame: map
63+
Value: true
64+
- Alpha: 0.25
65+
Autocompute Intensity Bounds: false
66+
Class: grid_map_rviz_plugin/GridMap
67+
Color: 200; 200; 200
68+
Color Layer: elevation
69+
Color Transformer: IntensityLayer
70+
Enabled: true
71+
Height Layer: elevation
72+
Height Transformer: Layer
73+
History Length: 5
74+
Invert Rainbow: false
75+
Max Color: 255; 255; 255
76+
Max Intensity: 2200
77+
Min Color: 0; 0; 0
78+
Min Intensity: 1400
79+
Name: GridMap
80+
Show Grid Lines: false
81+
Topic: /grid_map
82+
Unreliable: false
83+
Use Rainbow: true
84+
Value: true
85+
Enabled: true
86+
Global Options:
87+
Background Color: 255; 255; 255
88+
Default Light: true
89+
Fixed Frame: map
90+
Frame Rate: 30
91+
Name: root
92+
Tools:
93+
- Class: rviz/Interact
94+
Hide Inactive Objects: true
95+
- Class: rviz/MoveCamera
96+
- Class: rviz/Select
97+
- Class: rviz/FocusCamera
98+
- Class: rviz/Measure
99+
- Class: rviz/SetInitialPose
100+
Theta std deviation: 0.2617993950843811
101+
Topic: /initialpose
102+
X std deviation: 0.5
103+
Y std deviation: 0.5
104+
- Class: rviz/SetGoal
105+
Topic: /move_base_simple/goal
106+
- Class: rviz/PublishPoint
107+
Single click: true
108+
Topic: /clicked_point
109+
Value: true
110+
Views:
111+
Current:
112+
Class: rviz/Orbit
113+
Distance: 30173.89453125
114+
Enable Stereo Rendering:
115+
Stereo Eye Separation: 0.05999999865889549
116+
Stereo Focal Distance: 1
117+
Swap Stereo Eyes: false
118+
Value: false
119+
Field of View: 0.7853981852531433
120+
Focal Point:
121+
X: 5063.75927734375
122+
Y: 2195.057373046875
123+
Z: 3253.49755859375
124+
Focal Shape Fixed Size: true
125+
Focal Shape Size: 0.05000000074505806
126+
Invert Z Axis: false
127+
Name: Current View
128+
Near Clip Distance: 0.009999999776482582
129+
Pitch: 0.639797031879425
130+
Target Frame: map
131+
Yaw: 5.113188743591309
132+
Saved: ~
133+
Window Geometry:
134+
Displays:
135+
collapsed: false
136+
Height: 1136
137+
Hide Left Dock: false
138+
Hide Right Dock: true
139+
PlanningPanel:
140+
collapsed: false
141+
QMainWindow State: 000000ff00000000fd000000040000000000000247000003dbfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000007a000003db000000c700fffffffb0000001a0050006c0061006e006e0069006e006700500061006e0065006c0000000255000000ef0000006e00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000020c000001b10000000000000000fb0000000c00540065006c0065006f00700000000368000000b20000004500ffffff000000010000010f000002cafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000007a000002ca000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000073800000039fc0100000002fb0000000800540069006d00650100000000000007380000030700fffffffb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000014efc0100000001fb0000000800540069006d00650100000000000004500000000000000000000004eb000003db00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
142+
Selection:
143+
collapsed: false
144+
Teleop:
145+
collapsed: false
146+
Time:
147+
collapsed: false
148+
Tool Properties:
149+
collapsed: false
150+
Views:
151+
collapsed: true
152+
Width: 1848
153+
X: 72
154+
Y: 27

‎grid_map_geo/src/grid_map_geo.cpp‎

Lines changed: 10 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -141,7 +141,8 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path) {
141141
return true;
142142
}
143143

144-
bool GridMapGeo::initializeFromVrt(const std::string &path, const Eigen::Vector2d &map_center, Eigen::Vector2d &extent) {
144+
bool GridMapGeo::initializeFromVrt(const std::string &path, const Eigen::Vector2d &map_center,
145+
Eigen::Vector2d &extent) {
145146
GDALAllRegister();
146147
GDALDataset *dataset = (GDALDataset *)GDALOpen(path.c_str(), GA_ReadOnly);
147148
if (!dataset) {
@@ -175,18 +176,16 @@ bool GridMapGeo::initializeFromVrt(const std::string &path, const Eigen::Vector2
175176
std::cout << "Width: " << width << " Height: " << height << " Resolution: " << resolution << std::endl;
176177

177178
// pixelSizeY is negative because the origin of the image is the north-east corner and positive
178-
// Y pixel coordinates go towards the south
179-
int grid_width = extent(0)/std::abs(resolution);
180-
int grid_height = extent(1)/std::abs(resolution);
179+
// Y pixel coordinates go towards the south
180+
int grid_width = extent(0) / std::abs(resolution);
181+
int grid_height = extent(1) / std::abs(resolution);
181182
const double lengthX = resolution * grid_width;
182183
const double lengthY = resolution * grid_height;
183184
grid_map::Length length(lengthX, lengthY);
184185
std::cout << "length: " << length.transpose() << std::endl;
185186

186-
double mapcenter_e = map_center(0);
187-
double mapcenter_n = map_center(1);
188187
maporigin_.espg = static_cast<ESPG>(std::stoi(epsg_code));
189-
maporigin_.position = Eigen::Vector3d(mapcenter_e, mapcenter_n, 0.0);
188+
maporigin_.position = map_center.head(2);
190189

191190
Eigen::Vector2d position{Eigen::Vector2d::Zero()};
192191

@@ -197,13 +196,13 @@ bool GridMapGeo::initializeFromVrt(const std::string &path, const Eigen::Vector2
197196
grid_map_.add("elevation");
198197
GDALRasterBand *elevationBand = dataset->GetRasterBand(1);
199198

200-
Eigen::Vector2d center_px((mapcenter_e - geoTransform[0])/geoTransform[1], (mapcenter_n - geoTransform[3])/geoTransform[5]);
199+
Eigen::Vector2d center_px((map_center(1) - geoTransform[0]) / geoTransform[1],
200+
(map_center(0) - geoTransform[3]) / geoTransform[5]);
201201

202202
const auto raster_io_x_offset = center_px.x() - grid_width / 2;
203203
const auto raster_io_y_offset = center_px.y() - grid_height / 2;
204204
std::cout << "center_px: " << center_px.transpose() << std::endl;
205205

206-
207206
std::vector<float> data(grid_width * grid_height, 0.0f);
208207
const auto raster_err = elevationBand->RasterIO(GF_Read, raster_io_x_offset, raster_io_y_offset, grid_width,
209208
grid_height, &data[0], grid_width, grid_height, GDT_Float32, 0, 0);
@@ -227,8 +226,8 @@ bool GridMapGeo::initializeFromVrt(const std::string &path, const Eigen::Vector2
227226
static_transformStamped.header.stamp = ros::Time::now();
228227
static_transformStamped.header.frame_id = name_coordinate;
229228
static_transformStamped.child_frame_id = frame_id_;
230-
static_transformStamped.transform.translation.x = mapcenter_e;
231-
static_transformStamped.transform.translation.y = mapcenter_n;
229+
static_transformStamped.transform.translation.x = map_center(0);
230+
static_transformStamped.transform.translation.y = map_center(1);
232231
static_transformStamped.transform.translation.z = 0.0;
233232
static_transformStamped.transform.rotation.x = 0.0;
234233
static_transformStamped.transform.rotation.y = 0.0;

0 commit comments

Comments
 (0)