Skip to content

Commit d3782f3

Browse files
ataparlarataparlar
authored
fix(gnss_poser): added local coordinate system in UTM to gnss_poser (autowarefoundation#1227)
Signed-off-by: Ata Parlar <ataparlars5@gmail.com>
1 parent 8433c13 commit d3782f3

File tree

6 files changed

+60
-26
lines changed

6 files changed

+60
-26
lines changed

‎sensing/gnss_poser/README.md

+13-16
Original file line numberDiff line numberDiff line change
@@ -10,10 +10,10 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates
1010

1111
### Input
1212

13-
| Name | Type | Description |
14-
| ---------------- | ----------------------------- | ----------------------------------------------------------------------------------------------- |
15-
| `~/input/fix` | `sensor_msgs::msg::NavSatFix` | gnss status message |
16-
| `~/input/navpvt` | `ublox_msgs::msg::NavPVT` | position, velocity and time solution (You can see detail description in reference document [1]) |
13+
| Name | Type | Description |
14+
| ---------------- | ----------------------------- | --------------------------------------------------------------------------------------------------------------- |
15+
| `~/input/fix` | `sensor_msgs::msg::NavSatFix` | gnss status message |
16+
| `~/input/navpvt` | `ublox_msgs::msg::NavPVT` | position, velocity and time solution. [click here for more details](https://github.com/KumarRobotics/ublox.git) |
1717

1818
### Output
1919

@@ -27,14 +27,15 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates
2727

2828
### Core Parameters
2929

30-
| Name | Type | Default Value | Description |
31-
| -------------------- | ------ | ---------------- | ----------------------------------------------------------------------------------------------- |
32-
| `base_frame` | string | "base_link" | frame d |
33-
| `gnss_frame` | string | "gnss" | frame id |
34-
| `gnss_base_frame` | string | "gnss_base_link" | frame id |
35-
| `map_frame` | string | "map" | frame id |
36-
| `use_ublox_receiver` | bool | false | flag to use ublox receiver |
37-
| `plane_zone` | int | 9 | identification number of the plane rectangular coordinate systems (See, reference document [2]) |
30+
| Name | Type | Default Value | Description |
31+
| -------------------- | ------ | ---------------- | ------------------------------------------------------------------------------------------------------------------------------------------ |
32+
| `base_frame` | string | "base_link" | frame id |
33+
| `gnss_frame` | string | "gnss" | frame id |
34+
| `gnss_base_frame` | string | "gnss_base_link" | frame id |
35+
| `map_frame` | string | "map" | frame id |
36+
| `coordinate_system` | int | "4" | coordinate system enumeration; 0: UTM, 1: MGRS, 2: Plane, 3: WGS84 Local Coordinate System, 4: UTM Local Coordinate System |
37+
| `use_ublox_receiver` | bool | false | flag to use ublox receiver |
38+
| `plane_zone` | int | 9 | identification number of the plane rectangular coordinate systems. [click here for more details](https://www.gsi.go.jp/LAW/heimencho.html) |
3839

3940
## Assumptions / Known limits
4041

@@ -44,8 +45,4 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates
4445

4546
## (Optional) References/External links
4647

47-
[1] <https://github.com/KumarRobotics/ublox.git>
48-
49-
[2] <https://www.gsi.go.jp/LAW/heimencho.html>
50-
5148
## (Optional) Future extensions / Unimplemented parts

‎sensing/gnss_poser/config/set_local_origin.param.yaml

+3-3
Original file line numberDiff line numberDiff line change
@@ -2,10 +2,10 @@
22
ros__parameters:
33

44
# Latitude of local origin
5-
latitude: 40.816617984672746
5+
latitude: 40.81187906
66

77
# Longitude of local origin
8-
longitude: 29.360491808334285
8+
longitude: 29.35810110
99

1010
# Altitude of local origin
11-
altitude: 52.251157145314075
11+
altitude: 47.62

‎sensing/gnss_poser/include/gnss_poser/convert.hpp

+35-3
Original file line numberDiff line numberDiff line change
@@ -57,12 +57,12 @@ double EllipsoidHeight2OrthometricHeight(
5757
}
5858
return OrthometricHeight;
5959
}
60-
GNSSStat NavSatFix2LocalCartesian(
60+
GNSSStat NavSatFix2LocalCartesianWGS84(
6161
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg,
6262
sensor_msgs::msg::NavSatFix nav_sat_fix_origin_, const rclcpp::Logger & logger)
6363
{
6464
GNSSStat local_cartesian;
65-
local_cartesian.coordinate_system = CoordinateSystem::LOCAL_CARTESIAN;
65+
local_cartesian.coordinate_system = CoordinateSystem::LOCAL_CARTESIAN_WGS84;
6666

6767
try {
6868
GeographicLib::LocalCartesian localCartesian_origin(
@@ -99,7 +99,39 @@ GNSSStat NavSatFix2UTM(
9999
}
100100
return utm;
101101
}
102-
102+
GNSSStat NavSatFix2LocalCartesianUTM(
103+
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg,
104+
sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger)
105+
{
106+
GNSSStat utm_local;
107+
utm_local.coordinate_system = CoordinateSystem::UTM;
108+
try {
109+
// origin of the local coordinate system in global frame
110+
GNSSStat utm_origin;
111+
utm_origin.coordinate_system = CoordinateSystem::UTM;
112+
GeographicLib::UTMUPS::Forward(
113+
nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, utm_origin.zone,
114+
utm_origin.northup, utm_origin.x, utm_origin.y);
115+
utm_origin.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_origin, logger);
116+
// individual coordinates of global coordinate system
117+
double global_x = 0.0;
118+
double global_y = 0.0;
119+
GeographicLib::UTMUPS::Forward(
120+
nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm_origin.zone, utm_origin.northup,
121+
global_x, global_y);
122+
utm_local.latitude = nav_sat_fix_msg.latitude;
123+
utm_local.longitude = nav_sat_fix_msg.longitude;
124+
utm_local.altitude = nav_sat_fix_msg.altitude;
125+
// individual coordinates of local coordinate system
126+
utm_local.x = global_x - utm_origin.x;
127+
utm_local.y = global_y - utm_origin.y;
128+
utm_local.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger) - utm_origin.z;
129+
} catch (const GeographicLib::GeographicErr & err) {
130+
RCLCPP_ERROR_STREAM(
131+
logger, "Failed to convert from LLH to UTM in local coordinates" << err.what());
132+
}
133+
return utm_local;
134+
}
103135
GNSSStat UTM2MGRS(
104136
const GNSSStat & utm, const MGRSPrecision & precision, const rclcpp::Logger & logger)
105137
{

‎sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,8 @@ enum class CoordinateSystem {
2020
UTM = 0,
2121
MGRS = 1,
2222
PLANE = 2,
23-
LOCAL_CARTESIAN = 3,
23+
LOCAL_CARTESIAN_WGS84 = 3,
24+
LOCAL_CARTESIAN_UTM = 4
2425
};
2526

2627
struct GNSSStat

‎sensing/gnss_poser/launch/gnss_poser.launch.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@
1313
<arg name="gnss_frame" default="gnss"/>
1414
<arg name="map_frame" default="map"/>
1515

16-
<arg name="coordinate_system" default="1" description="0:UTM, 1:MGRS, 2:PLANE, 3:LocalCartesian"/>
16+
<arg name="coordinate_system" default="4" description="0:UTM, 1:MGRS, 2:PLANE, 3:LocalCartesianWGS84, 4:LocalCartesianUTM"/>
1717
<arg name="buff_epoch" default="1"/>
1818
<arg name="use_gnss_ins_orientation" default="true"/>
1919
<arg name="plane_zone" default="9"/>

‎sensing/gnss_poser/src/gnss_poser_core.cpp

+6-2
Original file line numberDiff line numberDiff line change
@@ -184,12 +184,16 @@ GNSSStat GNSSPoser::convert(
184184
GNSSStat gnss_stat;
185185
if (coordinate_system == CoordinateSystem::UTM) {
186186
gnss_stat = NavSatFix2UTM(nav_sat_fix_msg, this->get_logger());
187+
} else if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_UTM) {
188+
gnss_stat =
189+
NavSatFix2LocalCartesianUTM(nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger());
187190
} else if (coordinate_system == CoordinateSystem::MGRS) {
188191
gnss_stat = NavSatFix2MGRS(nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger());
189192
} else if (coordinate_system == CoordinateSystem::PLANE) {
190193
gnss_stat = NavSatFix2PLANE(nav_sat_fix_msg, plane_zone_, this->get_logger());
191-
} else if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN) {
192-
gnss_stat = NavSatFix2LocalCartesian(nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger());
194+
} else if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_WGS84) {
195+
gnss_stat =
196+
NavSatFix2LocalCartesianWGS84(nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger());
193197
} else {
194198
RCLCPP_ERROR_STREAM_THROTTLE(
195199
this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(),

0 commit comments

Comments
 (0)