Skip to content

Commit

Permalink
fix(gnss_poser): added local coordinate system in UTM to gnss_poser (t…
Browse files Browse the repository at this point in the history 
…ier4#1227)

Signed-off-by: Ata Parlar <ataparlars5@gmail.com>
  • Loading branch information
@ataparlar @boyali
ataparlar authored and boyali committed Oct 3, 2022
1 parent 2dd13a7 commit 1f0be11
Show file tree
Hide file tree
Showing 6 changed files with 60 additions and 26 deletions.
29 changes: 13 additions & 16 deletions sensing/gnss_poser/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,10 +10,10 @@ The `gnss_poser` is a node that subscribes gnss sensing messages and calculates

### Input

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

### Output

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

### Core Parameters

| Name | Type | Default Value | Description |
| -------------------- | ------ | ---------------- | ----------------------------------------------------------------------------------------------- |
| `base_frame` | string | "base_link" | frame d |
| `gnss_frame` | string | "gnss" | frame id |
| `gnss_base_frame` | string | "gnss_base_link" | frame id |
| `map_frame` | string | "map" | frame id |
| `use_ublox_receiver` | bool | false | flag to use ublox receiver |
| `plane_zone` | int | 9 | identification number of the plane rectangular coordinate systems (See, reference document [2]) |
| Name | Type | Default Value | Description |
| -------------------- | ------ | ---------------- | ------------------------------------------------------------------------------------------------------------------------------------------ |
| `base_frame` | string | "base_link" | frame id |
| `gnss_frame` | string | "gnss" | frame id |
| `gnss_base_frame` | string | "gnss_base_link" | frame id |
| `map_frame` | string | "map" | frame id |
| `coordinate_system` | int | "4" | coordinate system enumeration; 0: UTM, 1: MGRS, 2: Plane, 3: WGS84 Local Coordinate System, 4: UTM Local Coordinate System |
| `use_ublox_receiver` | bool | false | flag to use ublox receiver |
| `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) |

## Assumptions / Known limits

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

## (Optional) References/External links

[1] <https://github.com/KumarRobotics/ublox.git>

[2] <https://www.gsi.go.jp/LAW/heimencho.html>

## (Optional) Future extensions / Unimplemented parts
6 changes: 3 additions & 3 deletions sensing/gnss_poser/config/set_local_origin.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,10 +2,10 @@
ros__parameters:

# Latitude of local origin
latitude: 40.816617984672746
latitude: 40.81187906

# Longitude of local origin
longitude: 29.360491808334285
longitude: 29.35810110

# Altitude of local origin
altitude: 52.251157145314075
altitude: 47.62
38 changes: 35 additions & 3 deletions sensing/gnss_poser/include/gnss_poser/convert.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,12 +57,12 @@ double EllipsoidHeight2OrthometricHeight(
}
return OrthometricHeight;
}
GNSSStat NavSatFix2LocalCartesian(
GNSSStat NavSatFix2LocalCartesianWGS84(
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg,
sensor_msgs::msg::NavSatFix nav_sat_fix_origin_, const rclcpp::Logger & logger)
{
GNSSStat local_cartesian;
local_cartesian.coordinate_system = CoordinateSystem::LOCAL_CARTESIAN;
local_cartesian.coordinate_system = CoordinateSystem::LOCAL_CARTESIAN_WGS84;

try {
GeographicLib::LocalCartesian localCartesian_origin(
Expand Down Expand Up @@ -99,7 +99,39 @@ GNSSStat NavSatFix2UTM(
}
return utm;
}

GNSSStat NavSatFix2LocalCartesianUTM(
const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg,
sensor_msgs::msg::NavSatFix nav_sat_fix_origin, const rclcpp::Logger & logger)
{
GNSSStat utm_local;
utm_local.coordinate_system = CoordinateSystem::UTM;
try {
// origin of the local coordinate system in global frame
GNSSStat utm_origin;
utm_origin.coordinate_system = CoordinateSystem::UTM;
GeographicLib::UTMUPS::Forward(
nav_sat_fix_origin.latitude, nav_sat_fix_origin.longitude, utm_origin.zone,
utm_origin.northup, utm_origin.x, utm_origin.y);
utm_origin.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_origin, logger);
// individual coordinates of global coordinate system
double global_x = 0.0;
double global_y = 0.0;
GeographicLib::UTMUPS::Forward(
nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm_origin.zone, utm_origin.northup,
global_x, global_y);
utm_local.latitude = nav_sat_fix_msg.latitude;
utm_local.longitude = nav_sat_fix_msg.longitude;
utm_local.altitude = nav_sat_fix_msg.altitude;
// individual coordinates of local coordinate system
utm_local.x = global_x - utm_origin.x;
utm_local.y = global_y - utm_origin.y;
utm_local.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger) - utm_origin.z;
} catch (const GeographicLib::GeographicErr & err) {
RCLCPP_ERROR_STREAM(
logger, "Failed to convert from LLH to UTM in local coordinates" << err.what());
}
return utm_local;
}
GNSSStat UTM2MGRS(
const GNSSStat & utm, const MGRSPrecision & precision, const rclcpp::Logger & logger)
{
Expand Down
3 changes: 2 additions & 1 deletion sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,8 @@ enum class CoordinateSystem {
UTM = 0,
MGRS = 1,
PLANE = 2,
LOCAL_CARTESIAN = 3,
LOCAL_CARTESIAN_WGS84 = 3,
LOCAL_CARTESIAN_UTM = 4
};

struct GNSSStat
Expand Down
2 changes: 1 addition & 1 deletion sensing/gnss_poser/launch/gnss_poser.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@
<arg name="gnss_frame" default="gnss"/>
<arg name="map_frame" default="map"/>

<arg name="coordinate_system" default="1" description="0:UTM, 1:MGRS, 2:PLANE, 3:LocalCartesian"/>
<arg name="coordinate_system" default="4" description="0:UTM, 1:MGRS, 2:PLANE, 3:LocalCartesianWGS84, 4:LocalCartesianUTM"/>
<arg name="buff_epoch" default="1"/>
<arg name="use_gnss_ins_orientation" default="true"/>
<arg name="plane_zone" default="9"/>
Expand Down
8 changes: 6 additions & 2 deletions sensing/gnss_poser/src/gnss_poser_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -184,12 +184,16 @@ GNSSStat GNSSPoser::convert(
GNSSStat gnss_stat;
if (coordinate_system == CoordinateSystem::UTM) {
gnss_stat = NavSatFix2UTM(nav_sat_fix_msg, this->get_logger());
} else if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_UTM) {
gnss_stat =
NavSatFix2LocalCartesianUTM(nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger());
} else if (coordinate_system == CoordinateSystem::MGRS) {
gnss_stat = NavSatFix2MGRS(nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger());
} else if (coordinate_system == CoordinateSystem::PLANE) {
gnss_stat = NavSatFix2PLANE(nav_sat_fix_msg, plane_zone_, this->get_logger());
} else if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN) {
gnss_stat = NavSatFix2LocalCartesian(nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger());
} else if (coordinate_system == CoordinateSystem::LOCAL_CARTESIAN_WGS84) {
gnss_stat =
NavSatFix2LocalCartesianWGS84(nav_sat_fix_msg, nav_sat_fix_origin_, this->get_logger());
} else {
RCLCPP_ERROR_STREAM_THROTTLE(
this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(),
Expand Down

0 comments on commit 1f0be11

Please sign in to comment.