Skip to content

Commit 20c7a7f

Browse files
Jaeyoung-LimJaeyoung-Lim
committed
Add terrain loader node
1 parent 41b1186 commit 20c7a7f

File tree

6 files changed

+422
-6
lines changed

6 files changed

+422
-6
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/geo_conversions.h‎

Lines changed: 115 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,115 @@
1+
/****************************************************************************
2+
*
3+
* Copyright (c) 2023 Jaeyoung Lim. All rights reserved.
4+
*
5+
* Redistribution and use in source and binary forms, with or without
6+
* modification, are permitted provided that the following conditions
7+
* are met:
8+
*
9+
* 1. Redistributions of source code must retain the above copyright
10+
* notice, this list of conditions and the following disclaimer.
11+
* 2. Redistributions in binary form must reproduce the above copyright
12+
* notice, this list of conditions and the following disclaimer in
13+
* the documentation and/or other materials provided with the
14+
* distribution.
15+
* 3. Neither the name terrain-navigation nor the names of its contributors may be
16+
* used to endorse or promote products derived from this software
17+
* without specific prior written permission.
18+
*
19+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26+
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27+
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30+
* POSSIBILITY OF SUCH DAMAGE.
31+
*
32+
****************************************************************************/
33+
34+
#ifndef GEOCONVERSIONS_H
35+
#define GEOCONVERSIONS_H
36+
37+
#include <math.h>
38+
39+
class GeoConversions {
40+
public:
41+
GeoConversions();
42+
virtual ~GeoConversions();
43+
44+
/**
45+
* @brief Convert WGS84 (LLA) to LV03/CH1903
46+
*
47+
* @param lat latitude (degrees) WGS84
48+
* @param lon lontitude (degrees) WGS84
49+
* @param alt Altitude WGS84
50+
* @param x
51+
* @param y
52+
* @param h
53+
*/
54+
static void forward(const double lat, const double lon, const double alt, double &y, double &x, double &h) {
55+
// 1. Convert the ellipsoidal latitudes φ and longitudes λ into arcseconds ["]
56+
const double lat_arc = lat * 3600.0;
57+
const double lon_arc = lon * 3600.0;
58+
59+
// 2. Calculate the auxiliary values (differences of latitude and longitude relative to Bern in the unit [10000"]):
60+
// φ' = (φ – 169028.66 ")/10000
61+
// λ' = (λ – 26782.5 ")/10000
62+
const double lat_aux = (lat_arc - 169028.66) / 10000.0;
63+
const double lon_aux = (lon_arc - 26782.5) / 10000.0;
64+
65+
// 3. Calculate projection coordinates in LV95 (E, N, h) or in LV03 (y, x, h)
66+
// E [m] = 2600072.37 + 211455.93 * λ' - 10938.51 * λ' * φ' - 0.36 * λ' * φ'2 - 44.54 * λ'3
67+
// y [m] = E – 2000000.00 N [m] = 1200147.07 + 308807.95 * φ' + 3745.25 * λ'2 + 76.63 * φ'2 - 194.56 * λ'2 * φ' +
68+
// 119.79 * φ'3 x [m] = N – 1000000.00
69+
// hCH [m] =hWGS – 49.55 + 2.73 * λ' + 6.94 * φ'
70+
const double E = 2600072.37 + 211455.93 * lon_aux - 10938.51 * lon_aux * lat_aux -
71+
0.36 * lon_aux * std::pow(lat_aux, 2) - 44.54 * std::pow(lon_aux, 3);
72+
y = E - 2000000.00;
73+
const double N = 1200147.07 + 308807.95 * lat_aux + 3745.25 * std::pow(lon_aux, 2) + 76.63 * std::pow(lat_aux, 2) -
74+
194.56 * std::pow(lon_aux, 2) * lat_aux + 119.79 * std::pow(lat_aux, 3);
75+
x = N - 1000000.00;
76+
77+
h = alt - 49.55 + 2.73 * lon_aux + 6.84 * lat_aux;
78+
};
79+
80+
/**
81+
* @brief LV03/CH1903 to Convert WGS84 (LLA)
82+
*
83+
* @param x
84+
* @param y
85+
* @param h
86+
* @param lat latitude
87+
* @param lon longitude
88+
* @param alt altitude
89+
*/
90+
static void reverse(const double y, const double x, const double h, double &lat, double &lon, double &alt) {
91+
// 1. Convert the projection coordinates E (easting) and N (northing) in LV95 (or y / x in LV03) into the civilian
92+
// system (Bern = 0 / 0) and express in the unit [1000 km]: E' = (E – 2600000 m)/1000000 = (y – 600000 m)/1000000
93+
// N' = (N – 1200000 m)/1000000 = (x – 200000 m)/1000000
94+
const double y_aux = (y - 600000.0) / 1000000.0;
95+
const double x_aux = (x - 200000.0) / 1000000.0;
96+
97+
// 2. Calculate longitude λ and latitude φ in the unit [10000"]:
98+
// λ' = 2.6779094 + 4.728982 * y' + 0.791484* y' * x' + 0.1306 * y' * x'2 - 0.0436 * y'3
99+
// φ' = 16.9023892 + 3.238272 * x' - 0.270978 * y'2 - 0.002528 * x'2 - 0.0447 * y'2 * x' - 0.0140 * x'3
100+
// hWGS [m] = hCH + 49.55 - 12.60 * y' - 22.64 * x'
101+
const double lon_aux = 2.6779094 + 4.728982 * y_aux + 0.791484 * y_aux * x_aux +
102+
0.1306 * y_aux * std::pow(x_aux, 2) - 0.0436 * std::pow(y_aux, 3);
103+
const double lat_aux = 16.9023892 + 3.238272 * x_aux - 0.270978 * std::pow(y_aux, 2) -
104+
0.002528 * std::pow(x_aux, 2) - 0.0447 * std::pow(y_aux, 2) * x_aux -
105+
0.0140 * std::pow(x_aux, 3);
106+
alt = h + 49.55 - 12.60 * y_aux - 22.64 * x_aux;
107+
108+
lon = lon_aux * 100.0 / 36.0;
109+
lat = lat_aux * 100.0 / 36.0;
110+
};
111+
112+
private:
113+
};
114+
115+
#endif

‎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: 4 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -184,10 +184,8 @@ bool GridMapGeo::initializeFromVrt(const std::string &path, const Eigen::Vector2
184184
grid_map::Length length(lengthX, lengthY);
185185
std::cout << "length: " << length.transpose() << std::endl;
186186

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

192190
Eigen::Vector2d position{Eigen::Vector2d::Zero()};
193191

@@ -198,7 +196,7 @@ bool GridMapGeo::initializeFromVrt(const std::string &path, const Eigen::Vector2
198196
grid_map_.add("elevation");
199197
GDALRasterBand *elevationBand = dataset->GetRasterBand(1);
200198

201-
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], (map_center(0) - geoTransform[3])/geoTransform[5]);
202200

203201
const auto raster_io_x_offset = center_px.x() - grid_width / 2;
204202
const auto raster_io_y_offset = center_px.y() - grid_height / 2;
@@ -228,8 +226,8 @@ bool GridMapGeo::initializeFromVrt(const std::string &path, const Eigen::Vector2
228226
static_transformStamped.header.stamp = ros::Time::now();
229227
static_transformStamped.header.frame_id = name_coordinate;
230228
static_transformStamped.child_frame_id = frame_id_;
231-
static_transformStamped.transform.translation.x = mapcenter_e;
232-
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);
233231
static_transformStamped.transform.translation.z = 0.0;
234232
static_transformStamped.transform.rotation.x = 0.0;
235233
static_transformStamped.transform.rotation.y = 0.0;

0 commit comments

Comments
 (0)